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I. INTRODUCTION 


A. MOTIVATION AND BACKGROUND 


Land mines are an inexpensive and effective defensive means in wars. The 
problem with land mines is that they remain to be a threat when wars are over. 
International efforts are being made to ensure that land mines deployed in the future are 
equipped with a time-out device, and mine locations are properly recorded. While such a 
treaty may provide relief in the future, millions of land mines were planted all over the 
world as a result of wars and regional conflicts in the past. 

There are about 110 million land mines scattered around the world in more than 
60 countries --- most of them in the Third World [Ref. 1, 2, 3, 4, and 5]. These land 
mines kill about 10,000 and injure another 20,000 people (many of them are children) 
every year. Moreover, there are millions of acres of the US formerly used defense sites 
(FUDS) that are contaminated with unexploded ordnance (UXQ) as a result of military 
testing and training in the past [Ref. 6]. The contaminated land must be cleared inch by 
inch before transferring to civilian use. The difficulties of these clearing missions are in 
the variety of the objects to be identified and the diversity of the environments that are 


contaminated. 


B. OBJECTIVES 


As the military continues to downsize, the process of turning the land over to the 
civilian sector is sensitive, intensive, and costly. The aforementioned costs are both 
monetary and in some instance the loss of human life. One of the most complex 
problems is the clearing of UXO’s from the FUDS. 

The Department of Defense (DOD) has recently approved two organizational 
structures to confront the challenge of UXO remediation and wide-area de-mining. The 
objective of the first committee is to develop fully coordinated requirements driven 
research and development program for countermine, de-mining, site remediation, range 


clearance, and explosive ordnance disposal. Within the first committee there is a specific 


group focused on detection technology. The second committee will focus on current 
technologies and ways to improve in the future. One of the phases will examine current 
UXO remediation, active range UXO clearance and explosive ordnance disposal efforts. 
Hence, the UXO problem is serious and a highly visible issue within the DOD. The 
current approach to mine and UXO clearing is dangerous and labor intensive [Ref. 6]. In 
a typical UXO clearing scenario, Explosive Ordnance Disposal (EOD) technicians walk 
slowly and carefully over a contaminated field in an attempt to identify the presence of 
UXO’s that may be fully buried, half buried, or totally on the surface. UXO’s found on 
the surface are visually examined to determine their types and fuse mechanisms. If fuse 
mechanisms are recognized and the condition of the UXO permits (e.g., not rusted, 
decayed, or encased in soil), an effort 1s made to defuse UXO’s. UXO’s that cannot be 
defused are gathered at a safe location and are destroyed using shaped charges. 
Transporting any live UXO is extremely dangerous and the motion of the transport 
vehicle must be gentle. Moreover, buried UXO’s must be unearthed first. Therefore, 
UXO clearing consists of detection, identification, defusing, excavating, transporting, and 
disposal. Through robotics and the use of advanced technology it should be possible to 
make UXO clearing tasks safer, cost-effective, and more efficient. 

At the Naval Postgraduate School a team has been put together to develop a 
Semiautonomous vehicle or robot, which will survey possible contaminated areas for 
UXO’s. The “rotary” class vehicle by its very design is capable of independent driving 
and steering with each wheel. Hence, rotary vehicles are capable of stronger torque and 
traction than most other vehicles on rugged terrains. Rotary vehicles with two wheels (on 
a very smooth surface) have been shown to possess the aforementioned enhanced torque 
and traction — with an increase in the number of wheels on the vehicle the capabilities are 
significantly improved. The vehicle needs to be highly mobile and capable of producing 
very fine motion to negotiate and search contaminated sites. To fulfill the requirement of 
the special wheel architecture, the semiautonomous vehicle, called Shepherd (a rotary 
vehicle), is presently under development. Shepherd possesses stronger torque and 


traction on rugged terrain, because of its special architecture (1.e., useful for the 


mine/UXO. Mission). The name “Shepherd” was given to the vehicle because of the 
protective function of the “shepherd” in many ancient cultures—hopefully this Shepherd 
will also save lives. Shepherd (Figure 1.1) is a four-wheeled vehicle with independent 
steering and driving capabilities. The four-wheel independent driving and steering 
capability provides Shepherd a high level of mobility and preciseness in motion control 


[Ref. 7]. 





Figure 1.1: A “rotary” vehicle: Shepherd. 


The fundamental objective of this thesis research project is to assist in the 
construction of a user-friendly and high-precision semiautonomous robotics tool to help 


the unexploded ordnance (UXO) and mine clearing mission. 


This thesis will examine the following research areas: 


What kinematics algorithms must be developed to support a vehicle with three 
degrees of freedom of motion? The aforementioned algorithms must support 
highly flexible, controlled, and precise motion. 


What types of controls are required to ensure the optimal mix of driving and 
steering resources? Moreover, what must be done to ensure that all the 
resources complement? 


How can the knowledge gained in the aforementioned research areas be used 
to develop searching motion? 


How should the hardware and software systems be implemented to support 
the aforementioned goals? 


How can human operators remotely control the vehicle through an appropriate 
interface? 


CC ORGANIZATION 


This chapter provides a general overview of traditional and current techniques for 


identifying unexploded ordnance. Chapter II provides the System Overview and 


illustrates the concept of motion control. Chapter III describes the Shepherd Mobile 


Platform and the On-Board Computer System. Chapter IV presents the Shepherd 


Software Description and places great emphasis on the Shepherd Real-time Kernel 


(SRK). In Chapter V the results of experiments and testing motion control are presented. 


Chapter VI explains the current motion modes and Chapter VII summarizes the thesis. 


II. SHEPHERD SYSTEM DESIGN 


A. SYSTEM OVERVIEW 


In consideration of aiding the UXO task, what type of vehicle should be 
developed? This vehicle will face difficulties of clearing missions in the variety of the 
objects to be identified and the diversity of the environments that are contaminated. This 
vehicle has to have precise and smooth motion, display motion flexibility, and contain 
robust motion in varied environments such as soft soil and rough terrain. 

The vehicle must be capable of precise and smooth motion while searching for 
UXO’s. The very nature of trying to locate UXO’s should be meticulous and cautious. 
Haphazzard and jerky motions could contribute to lost of the vehicle due to unwanted 
detonations. Also, motion flexibility is absolutely necessary. This will enable different 
approaches or techniques for locating UXO’s. Finally, the motion exhibited must be 
robust and stable due to the nature of UXO environments. While traversing these 
environments, the vehicle should not lose its precise, smooth, and flexible motion 
characteristics. For these reasons, a rigid body vehicle with at least 2 steerable wheels 
capable of semiautonomous or autonomous motions and equipped with sensors for 
detecting UXO’s was considered at the abstract level. 

So, the rotary vehicle platform was chosen, with the addition of four steerable and 
drivable wheels and a powerful computer system for control. The four wheels have thick 
tires and each contains two motors, one for driving and one for steering. Because of this, 
three degrees of freedom motion is possible which allows for motion flexibility. The 
independently driven four wheels aids in providing stronger traction than any other 
wheeled vehicle allowing for the negotiating of uneven slopes , soft soil, or rough terrain. 
The vehicle itself also provides the capability for further expansion of the system which 
will give it the full capability of fulfilling all aspects of the UXO mission. Chapters III 
and IV expand on the details of the system architecture regarding the hardware and 


software of this rotary vehicle. 


B. MOTION MODES 


Due to the uniqueness of Shepherd’s independent 4-wheel motion of 360 degrees, 
several modes of motion are possible. The possible vehicle motions are: 

e Tangential -- the vehicle’s change in direction of movement is equal to its 

change in heading. 

e Constant orientation -- the vehicle’s heading is constant regardless of the 

change in the direction of movement. 

¢ Complex which falls into neither of the above. 

e Rotation 
At this stage of development of Shepherd the constant orientation, complex, and rotation 
motion modes have been implemented with work proceeding on the tangential motion 
mode. 

The user interface menu has a list of specific motions designed for the vehicle, 
which encompasses the typical vehicle motions. This list includes 1-Stop, 2-Straight 
motion (autonomous), 3-Straight motion by joystick, 4-X Y-motion by joystick, 5-Rotate, 
6-Sinusoidal, 7-Tornado (external), 8-Tornado (internal), 9-Tangential, O-Exit, a- 
Tangential motion II, and t-Test motion. The sinusoidal motion is an implementation of 
the constant orientation while the tornado motions are an implementation of the complex 
vehicle motion. Both tangential motions are an attempt to implement the tangential 
vehicle motion. Further coverage will be given to several of these motions in more 


detail. 


Ill. SHEPHERD SYSTEM HARDWARE 


ae OVERVIEW 


The Shepherd system hardware consists of the mobile platform and the shepherd 
on-board computer system. The mobile platform is the “mechanical” part of Shepherd, 
which provides motion and is directed by the on-board computer system. The Shepherd 
on-board computer system provides the computing power required controlling and 
directing Shepherd. Figure 3.1 provides a global perspective of the Shepherd System 
Hardware. Shepherd has four wheels, which are controlled independently. Each wheel 
has two motors; one for driving the wheel and the other for steering. Moreover, the 
Steering capability for each wheel exceeds 360 degrees. The maximum driving speed 
(determined empirically) is approximately 87 centimeters/second. The unique mix of 
driving and steering capability is what provides the challenge and motion flexibility of 
this vehicle. 

Shepherd has a mass of 150 kilograms and is built to form a square chassis 
(frame). Shepherd’s wheels are centered on the corners of the square leading to very 
elegant calculations, as you will see later. Shepherd’s Alternating Current (AC) electric 
motors are powered by 12 batteries, which are charged by an external AC source through 
converter. Figure 3.1 is also a transparent view of Shepherd from above which shows the 
Central Processor Unit (CPU), input/output (I/O) boards, servoamplifiers, batteries, and 


Wheel unit assembly. 


B. THE MOBILE PLATFORM 


The mobile platform consists of the vehicle body. The vehicle body includes the 
vehicle’s frame, motors, encoders, servocontrollers, gears, wheels, tires, and power 


supply. The on-board computer system is not considered part of the mobile platform. 


CPU and YO — Servoamplifiers Batteries 
Wheel assembly 
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Figure 3.1: A transparent view of Shepherd from above. Showing the “relative” location of the 
CPU, and I/O boards, servoamplifiers, batteries, and wheel assembly. The relative position of the 
vehicle components is subject to change as the vehicle is modified. 


i: Motors and Encoders 


The eight motors and their corresponding shaft encoders used in Shepherd are 
from Yamayo Electric, Inc. These motors allow Shepherd to reach a “theorectical” 
maximum driving speed of 4 kilometers per hour (km/h), and a rate of | revolution per 
second about the steering axis (Ref. 7). Figure 3.2 provides the characteristics for the 


driving and steering motors. 





Servomotor Characteristics 
Driving Motor Steering Motor 














Maximum 3.84 N-m 0.98 N-m 
Nominal 3000 rpm 3000 rpm 
Maximum 4500 rpm 











Rotation Rate 


60 X 123.5 mm 54 X 86 mm 


Power (AC) 400 W 100 W 


Figure 3.2: Servomotor characteristics for Shepherd. 


Figure 3.3 illustrates the relative motor (M1-M7) position on the vehicle and the 


general orientation of the vehicle (e.g., front). 


Shepherd Rotary Vehicle 
Motor diagram 





Figure 3.3: Shepherd motor diagram 


De Servomotor Controllers 


The servomotor controllers actually provide the commanded voltage and current 
to the driving and steering motors to effect motion. The importance of the servomotor 
controllers can not be understated; the values written to the digital output board (and read 
from the digital input board) are within the acceptable range of the controllers. Also, the 
interface specification matches the range for the motors used on Shepherd and input 
signal voltage corresponds to the range allowed for the driving and steering motors. Later 
sections of this document will show that the voltage produce by the batteries 1s 
approximately 144 volts, which is within the acceptable range of the controllers. Figure 


3.4 contains the characteristic and interface data for the servomotor controllers. 


Servo Motor Controller Specifications 
Motor Capacity 400 W 


Interface Specification 3000 rpm/5000 rpm 











Control Method 


Input Control Voltage DC + 120~150 V 
Input Signal Voltage DC +/- 10V 


Figure 3.4: Servomotor controller’s characteristic and interface data. 











3. Gears 


Shepherd’s reduction gear system contains flat gears, planetary gears, and bevel 
gears. This gear configuration has a 1:50 gear ratio for both driving and steering [Ref. 8]. 
Figure 3.5 provides a transparent view of the flat gears in the wheel assembly. Due to the 
gear configuration, when the wheels are used for steered then some driving is also 


initiated. The aforementioned driving is cancelled by applying the required amount of 
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“opposite” driving. And this “opposite” driving is based on the 1:50 gear ratio and is 
handled in the Shepherd code (Appendix J ,Consolidated header files, line 360). 

Figure 3.6 is a side cut away of the wheel assembly. This cut away shows the 
gears involved in transferring force from the motors to the wheels. Gear ratio and force 
calculations can be obtained from [Ref. 8]. Also, mounted one of the flat gears is a “hall” 


sensor, which Shepherd uses to determine if wheel is aligned. 





Figure 3.5: Flat gears in the wheel assembly (transparent view from the top) 


1] 





Driving Motor 
Steering 
Motor 


Figure 3.6: Side cut away of the motors and gears used in the Shepherd driving and 
steering mechanism (diagram not to scale). 


4. Wheels and Tires 


Shepherd’s wheel diameter is 400 millimeters (mm), and the suspension travel 
range is 100 mm. Shepherd has somewhat ruggedized tires, with a tire friction factor of 
.5. The maximim tire pressure has been calculated as 49.8 pounds per square inch (psi); 


where 36 psi is currently used. 


a: Power Supply System 


Shepherd’s power supply consists of twelve (12-volt) batteries connected serially. 
The voltage generated from the batteries is between 144-150 volts (within the servomotor 
specifications). The batteries have been used for periods up to two (2) hours without any 
noticeable degradation of performance. Figure 3.7 shows the switches required for the 
operational settings of charge, run (battery), run (external) and run (external/charge). The 


following are valid switch settings for Shepherd. 


e Charge: 1 -SW-C OFF 
2 -SW-B ON 
3 -SW-A ON 


e Run (battery): 1 -SW-A OFF 
2 -—SW-C ON 
3 —SW-B ON 


e Run (external): 1 -SW-B OFF 
2 —-SW-C ON 
3 —-SW-A ON 


e Run (external/charge): 1 -SW-A ON 
2-SW-B ON 
3 -SW-C ON 


Shepherd Rotary Vehicle 
Power Supply 


Shepherd 
(s) AC/DC Converter Rotary Robot 





Figure 3.7: Shepherd power supply switch diagram 
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Figure 3.8: Simplified schematic diagram of Shepherd power supply. Showing both an external 
AC source and the 12-volt batteries serially connected. 


An AC source (115-Volts) can be used to run Shepherd or to charge the batteries 
(accomplished by the AC/DC converter). Figure 3.8 1s a global schematic of the 
Shepherd power supply [Ref. 9]. 


C; SHEPHERD ON-BOARD COMPUTER SYSTEM 


The Shepherd vehicle’s system design is illustrated by Figures 3.9, and is broken 
down into the hardware and software components both of which will be explained in 
greater detail in later. The hardware system is a combination of the mobile platform, an 
on-board computer system, servo drivers, batteries, and a laptop computer for a real-time 
V/O device. The computer system consists of a Taurus board housing two Motorola 
CPUs which are 68040 and 68030, a digital to analog board, a digital input board, a 
digital output board, a digital counter board; and a Versa Module European bus 
(VMEbus) based on Motorola architecture. Servo-controllers are connected to these I/O 


boards. Motor encoders are connedcted to the counter board. 
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Figure 3.9: Diagram of the Shepherd on-board computer system. 
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1. Taurus Board 


The Taurus is a dual-processor, and dual bus architecture, VME single slot, single 
board computer [Ref. 11]. The primary computing engine is a Motorola 68040 processor 
running at 25 MHz. The second processor on Taurus is the Motorola 68030. Although, 
taurus supports several real-time operating systems, an in-house operating kernel, SRK, 
was developed (chapter IV). Taurus also takes advantage of the direct memory access 
(DMA) functions provided by Ethernet, SCSI, and Intelligent Serial Controllers to DMA 
into main direct access memory (DRAM) through an isolation gateway between the 
M68040 bus and the M68030 bus. Moreover, Taurus acts as a fully functional VMEbus 
controller and may operate in Slot 1 of the VMEbus back plane (this is the case for 
Shepherd). Hence, the Taurus board is a powerful VMEbus engine and supports the 
requirements for a real-time operating system and a completely self-contained computing 


environment. Taurus features: 


e 25 MHz M68040 Processor 

e Burst Transfers 

e Ethernet and SCSI with on chip DMA 

e 16 Megabytes of DRAM main Memory 

e 4 Megabytes of EPROM 

e | Megabyte of Flash EPROM 

e 25 MHz M68030 Processor 

e 6 Serial Ports: 4 (RS-232-D Intelligent Ports with DMA), and 2 using a 
68c681 device 

e 32 Parallel /O 

e 11, 16 bit Timers (cascadeable into combinations of up to 80 bits) 

e Interprocessor Mailbox 

e Dynamic Bus Sizing 

e Real-time clock with battery back up 

e Watchdog Timer and 8 KB of battery back-up Static RAM 


Why was the Taurus board chosen for this project? In addition to the 
aforementioned characteristics, the board uses the M68040 processor. In previous 
development of the Yamabico-11 [Ref. 12] robot, the M68020 was used. Motorola 
claims that its M68000 series chips are backward compatible. This research has 
concluded that this is mostly true, however on some key issues (math functions and 
assembly code) this has not proven to be the case. These issues will be revisited in 
chapter IV. Again a key asset of the Taurus board is how elegantly and logically internal 


and external interrupts are handled (Figure 3.10). 


VMEbus Interrupts 


U 


Interrupts from 
On-Board Devices ISM _ | >| vicosswvices | > 
-SCSI Interrupt Steering Mechanism IRQ-6 
-Ethernet 
M 68030 
IRQ-x 


Figure 3.10: Interrupt handling diagram for the Taurus board [Ref. 11] 





The Taurus board's communications facility adds flexibility to the implementation 
of RS-232 or Ethernet. The M68040 chip is a very versatile processor and powerful 
processor, and can perform 14 different operations at a given time. That is 6 operations 
by the Integer Unit (IU), 3 operations by the Floating-Point Unit (FPU), 4 by the Memory 
Management Unit (MMU), and a Bus interface operation. The number of timers on the 
Taurus board is also of a great benefit, moreover Timer 5 will be discussed in chapter IV. 
The M68040 is highly parallel, with a 6-stage integer pipeline, that when filled, will 
execute an instruction for every clock cycle. Moreover, each of the MMUs can 


accomplish a cache access and address translation concurrently. 
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Ze Digital To Analog Board 


The Acromag Series 9210 Analog Output Board (AVME9210) provides the 
means for connecting and driving analog circuits with outputs from the VMEbus for the 
Shepherd system [Ref. 13]. The board has 8 channels; each channel has a 12-bit 
resolution. A DAC per channel is used for signal accuracy. The DACs are set up to 
accept either straight binary or two’s compliment data. The board has five programmable 
ranges for output voltages; however, +/- 10-Volts will be used with shepherd because it is 
a direct mapping to the maximums for the servomotors inputs. Characteristics of the 
AVME9210 are as follows: 


e 12 bit output resolution 

e individual DAC per channel 
e 8 channels per board 

e Byte or Word data transfers 
e Power up reset 


e Pass/Fail status indicators on the front panel 





Figure 3.11: DAC data register, 8 channels of output. Each channel is a digital to analog converter. 
A two byte address is reserved for each data register. 
















A single channel represents a driving or steering motor in Shepherd [Appendix E, 


Motor.c, line 228]. The status control register controls the pass/fail light. The Shepherd 
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code toggles the pass fail light in some instances to ensure the system and code are 
functioning properly. There is a memory location for the board status indicator flags and 
reset. This memory location is one byte in length and is located at base address +81H. 
During the early manual testing this status control register was used exclusively to 
accomplish resetting the AVME9210. Also, each of the aforementioned two byte DAC 


data registers (Figure 3.12) are set up as follows: 


esp] is 


ro 
fou | pio | pe | pe | py | ps | vs | ve | os | oe Lor [oo [x |x [x |x 


12 bits 4 bits 


Bit DAC Data Undefined 


Figure 3.12: DAC data register (16 bit). Most significant bit (MSB) and least significant bit (LSB). 
The 12 bits are a direct mapping of input values to the board, integer range [1023, -1024]. 





The AVME9210 is located at base address Oxffff0400 and is represented in the 
Shepherd code by the “label” VME9210. 


ot ee 
Address Address 
VME9210 + 0x0082 VME9210 + 0x008A 
VME9210 + 0x0084 VME9210 + 0x008C 
VME9210 + 0x0086 VME9210 + 0x008E 
VME9210 + 0x0088 VME9210 + 0x0090 


Figure 3.13: AVME9210 address to “physical”? motor mapping. 












3. Digital Input Board 


The Acromag Series 9421 Isolated Digital Input Board (DIB) provides the means 
for connecting the Digital DC inputs the VMEbus for the Shepherd system [Ref. 14]. 
The DIB board isolates all digital inputs from the VMEbus for up to 250V AC, or 350V 
DC on acontinuous basis (falls within the constraints of the Shepherd servomotors). The 
pass/fail light on this board is similar to the one used in the digital to analog board 
previously mentioned. And the DIB also has the input channel on light as well. The 
board has 64, | bit channels configure as four, 16 bit words. The inputs can be bi-polar 
(with polarity being +/- or -/+ at either end of the channel). The bi-directional polarity 
allows Shepherd to use this for changing the direction of wheel driving or the direction of 
steering with a change of input polarity. The DIB has the base address of OxffffO000 and 
the “label” VME9421 is used in the code [Appendix J, Consolidated header files, line 
386]. 


4. Digital Output Board 


The Microsystems International Corporation 32-bit Optically Coupled Digital 
Output Board (VMIVME-2170A) consists of VMEbus compatibility logic, data output 
control logic, four 8-bit output registers, and 32 bits of isolated outputs. The VMEbus 
logic contains address decoding logic and data transfer contro] logic, which provides for 
8- or 16- bit data transfers in the “‘short” I/O address space. The data output control logic 
selects byte or word transfers to the 32 optically isolated channels. The Shepherd 
research group spent many hours attempting to master this logic—a key problem was 
how to determine where the least significant value was for each data register. However, 
Thorsten Leonardy’s efforts paved the way for a consistent and logical method of writing 
to the aforementioned registers. From this the Shepherd group was able to selectively 
choose combinations of motors for steering and driving [Appendix J, Consolidated 
header files, lines 181-192]. Figure 3.14 shows the register bit definitions [{ Ref. 15]. 
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Figure 3.14: Register Bit Definitions 




















The output board has the base address of Oxffffff00, which is represented in the 


code by the “label” VME2170. 









VME2170 Bit Assignment 
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Figure 3.15: VME2170 bit assignment. Note 3 bit assignment for driving or steering motor selection. 


Figure 3.15 indicates that “masks” could be written to the VME2170 base address 
and used for motor selection. Hence, writing a mask for OxO0000004 would select, motor 
1 (M1) in wheel | for driving. And writing a mask for 0x00004000 would select, motor 
5 (M5) in wheel 1 for steering. Moreover, using a mask for 0x00924924 all motors can 


be selected (using a “logical And” on the true values for each motor selected). 


S: Counter Board 


Shepherd uses the Green Spring IP-Quadrature Four Channel Quadrature Decoder 
or counter board [Ref. 16]. The counter board reads the signal produced by the encoders 
(see section HI.B.1 of this document); this signal 1s “index” pulse once per revolution to 
provide absolute position information. There are four channels on the counter board; 
each channel has three inputs. The inputs are normally called X, Y, Z, and the board 
inputs are X and Y. Z is the control or index input. Each channel has a 24-bit up/down 
counter block, 24-bit capture/match register and a 24-bit output latch allow for an 
accurate “on the fly” reading of Quadrature position values. The up/down count direction 
in the counter board is controlled by the relative phase of the X and Y inputs. Count 
direction can be reversed by: reversing the mechanical motion direction, reverse the 
connections for X and Y, reverse the X polarity bit, or reverse the Y polarity bit. 
Shepherd uses a function called “readEncoders” which is an excellent example of 
completing three consecutive 8-bit reads from the counter board and catenation the 8-bit 
segments into a single 24-bit position reading [Appendix E:, Motor.c, lines 300-356 and 
606-620]. The counter board’s base address address is Oxffff6000, and is represented by 
the label VMECTRI in the Shepherd code. 
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IV. SHEPHERD SOFTWARE SYSTEM 


A. OVERVIEW 


The Shepherd software system consists of the software development environment 
(including GCC version 2.7.2.1 compiler), Shepherd Real-time Kernel (SRK), and the 
firmware on the Taurus board. Shepherd software is developed in the “C” language on a 
Unix workstation, and the code is cross-compiled using switches with the GCC compiler 
to ensure viability on the Taurus board (step by step instructions are in the Shepherd 
Operating Manual, Appendix K). The code is in “S” record format and is transferred (via 
Ethernet) to a laptop computer, which is used as the user interface. When the user 1s 
ready to test or run a program, it is then download via RS232 to the RAM on the Taurus 
board using the Taurus bug or firm-ware on the Taurus board. The SRK has real-time 
timer control. The timer interrupts are set for 10 milliseconds, but can be modified to 
suite user needs and requirements. SRK’s central motion contro] sections are shepherd.c, 
user.c, and the driver routine (all of which will be discussed in great detail later). The 
user interface viewed from the laptop computer is also generated from the code in user.c 
and other I/O code segments. The overall software environment and firmware works 
together to form a tightly coupled and low overhead “operating kernel” that is the SRK. 
The use of the SRK allows the user to control (i.e., timer interrupt, motion control, and 


user interface) the Shepherd rotary vehicle. 


B. SOFTWARE ENVIRONMENT 


The software environment (minus the SRK) includes the following: Bug monitior, 


S-records, and Software development environment. 


Ie Bug Monitor 


The “taurus bug” is a powerful debugging and evaluation tool, and is firmware on 
the Taurus board [Ref. 20]. It has facilities for loading and executing user programs 
under complete operator control and evaluation (and is used extensively in Shepherd 
development). The “taurus bug” includes commands that allow the user to display 


memory, modify memory, set and remove break points, an assembler/disassembler, and a 
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system self test capability that verifies system integrity upon power up. The “taurus bug” 
also has various routines to handle some I/O, string functions, and data conversion via the 
TRAP #15 handler on the Taurus board (used in Appendix C, lines 535-548). Moreover, 
on power up, all static variables are set to default states, the break point table is cleared, 
all target registers invalidated, I/O character queues cleaned, the vector interrupt table is 
written to RAM, and all on-board devices (serial ports, timers, etc.) are cleared or reset. 
Taurus bug also has a system reset and abort feature. The system reset completely re- 
initializes the board and the abort feature captures a snapshot of the processors present 
state—allowing the use of stack pointers, and the program counter to help determine 
errors (the hardest way to debug). The “taurus bug” was a very valuable tool; however, 
at times it was difficult to use. And tracing through assembly code to resolve a problem 
using the “Trace” function and the symbol table can take numerous man-hours —without 
immediately yielding a positive result. A very important function of “taurus bug” is the 
loading capability. The use of the “Lo” command to place executable code in memory is 
key in the development process. The “S” records are downloaded to the Taurus board in 


this manner. 


a “*S”? Records 


The S-record format was devised by Motorola for output modules. Its key 
purpose was encoding programs or data files in a printable format for the transportation 
between computer systems. Hence, providing a way of visiually monitoring the 
transportation processs and a method of quicky editing the code if required. S-records 
are character strings made of several fields which identify the record type, record length, 


memoery address, code data and checksum (see Figure 4.1). 


i 
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Figure 4.1: S-record content chart [Ref. 20]. 
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S-records module may contain the following types (and many more): SO (header 
data), record containing address where code is to reside in memory (S1, S1, or S3), S5 
(the number of records transmitted per block), and the termination record (S7, S8, or S9). 


A typical S-record might look like this: 


S00600004844521B 
$113000284F245F2212226A000424290008237C21 
S11300100002000800082629001853812341001813 
S9030000FC 


A detailed byte wise explanation of S-records is contained in Ref. 20. The S-records for 
shepherd’s development are generated during the linking and loading process. A special 
switch is used that allows the creation of a file named ‘“shepherd.TXT”. The 


“shepherd. TXT” file contains the S-records to be downloaded. 
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5: Software Development System 


As mentioned in this section's overview the compiler used is the GCC 2.7.2.1. 
This compiler posed many problems for SRK development. One of the biggest obstacles 
was the passing of “composite” structures. Structure values had to be placed into dummy 
variables (Appendix D, lines 138-140) in order to get the code to execute. This was 
especially odd because the code compiles, but will not execute (if the dummy variables 
are not used). Also, several of the compiler switches that supposedly allow mathematical 
code (that was previously valid for the M68020 with math co-processor) would simply 
not compile. An inordinate amount of time was spent trying to resolve this issue because 
one of the initial precepts of the project was that old code from the Yamabico-11 robot 
system would be portable. There are still anomalies that work arounds were developed 
for. For instance, there is a “square root” function that compiles and works in every 
environment (other compilers) yet would never function when compiled using GCC 
2.7.2.1. Moreover, because the compiler does not have libraries for I/O or standard math 
functions, these had to be derived to support the M68040 and Taurus board (again taking 
an inordinate amount time). The compiler did serve its purpose—because its freeware 
(the budget did not allow for commercial compilers). 

One switch that did work was —m68040, which allowed the generation of M68040 


specific code. The “makefile” makes use of the -m68040 (segment below): 


shepherd.o : shepherd.c 
gcc -c -m68040 -o shepherd.o shepherd.c 


timer.o : timer.c 


gcc -c -m68040 -o timer.o umer.c 


USCEO = USEL.-C 


gcc -c -m68040 -o user.o user.c 


motor.o : motor.c 


gcc -c -m68040 -o motor.o motor.c 
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The early testing also required a complete understanding of the “a.out” generated 
from the assembler and the link editor (the link editor makes “a.out’’ executable files). 
The “a.out” (Figure 4.2) consists of: a header, program text, program data, text and data 
relocation information, a symbol table, and a string table (the last three sections may be 
omitted if the program is loaded with the -s option. All of the aforementioned 
information was useful because our earliest a.outs were in the wrong format, hence not 


useable by the M68040 due to using non-functioning compiler switches. 


Magic Number 7 size text 3 size data 3 Other Header data 


BSS STACK 
SEG MENT 





Figure 4.2: A generic A.OUT format. Note many features of the ACOUT 
are left off, such as symbol table, entry point, dynamic, and machine type. 


The final element to be discussed is how the link editor is used. First, we must 
discuss the DRAM Memory map (Figure 4.3). The Taurus board documentation [Ref. 
21] warns that accessing the memory below $10000 (Hex), hence the memory the format 


used in Figure 4.3 with 16 Megabytes as the upper bound. 


2) 


DRAM Memory Map 


$100000 
or 
16 MB 


Region Available for 
User program 


$10000 Start user DRAM 
Region used by 
“taurus bug” 
$00 — Start DRAM 





Figure 4.3: DRAM memory map. It should be noted that to be able to write to 
the DRAM the Parity ISM register must be disabled or the block- fill command 
(taurus bug) used to clear the region required [Ref . 12, and 22]. A detailed 
example is in the Shepherd Operating Manual (Appendix K). 


Again the Shepherd makefile 1s illustrative of the linking process: 


comp: startup.o shepherd.o timer.o serial.o math.o utils.o utils030.0 user.o\ 


motor.o movement.o 


Id -Ttext 0x10000 -Tdata 0x20000 -Tbss 0x30000 -Map shepherd.map 
oformat srec\ 
-o shepherd. TXT startup.o shepherd.o timer.o serial.o math.o utls.o\ 


utilsO30.0 user.o motor.o movement.o 


The make file shows the text segment of the code being loaded at 0x10000, the data 
segment of the code loaded at 0x20000, and the upper bound for the data at 0x30000. 
Hence, the code is loaded within the parameters required by the memory map. Earlier it 
was mentioned that the S-records were generate by the linker—the “oformat srec —o 
shepherd. TXT” generates the required S-records for download to the laptop PC. If the 


“oformat srec” switch is not used then the standard a.out will be generated. The —Map 
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switch also allows the user to generate a symbol table (called shepherd.map here) for use 


in debugging. At this point all the underlying structure for SRK development is in place. 


Cc. SHEPHERD REAL-TIME KERNEL (SRK) ARCHITECTURE 


The SRK includes the Timer control, Motion control, and the User interface. 


Figure 4.4 below illustrates the exact architecture of the motion control part of the 


system. 
motionMode 
desredSp es 
bodyMotion ee | desiredAngles 9 | 
thetaDot : 
wheelMotion driveMotors 
SteerDigas SpeedDigits 
pyStck x pySnck.y 







actualAngles 


Joy Stick 
(physical object) 


Servo Drivers 
Moters 


Shaft Encoders 










computeActualAngies 
com pute Actual Rates 


(takes derwanve) 


Figure 4.4: Shepherd Motion Control Architecture 


1. Timer Control 


The timer control is a very integral part of the system. As was stated earlier SRK 


has a real-time modifiable timer control. This timer control 1s made possible through the 
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use of the Taurus board’s AM9513A Counter/Timer. This Timer/Counter provides five 
16-Bit general purpose counters and uses a 4-Mhz oscillator as a clock input with the 
outputs connected to the Taurus Interrupt structure for processing. The Timer 5 group of 
the AM9513 device was utilized due to it’s compatibility with the 68040 Processor and 
contains it’s own timer handler. 

The main routine in controlling the timing and setting the interrupts is located in 
the timer.c file along with the header file in timer.h (Appendix F). This timer 1s 
initialized and started in the main shepherd routine. It continuously provides a 10 
millisecond interrupt until the program is terminated. However, this value could be 
modified. This was made possible by manipulating the data port values of the AM9513-1 
device and multiplying those values by factors of 10000, 1000, 100, 10, or 1 to obtain a 1, 
0.1, 0.01, 0.001, or 0.0001 second interrupt in that order. The accuracy of this timing 
was tested using an oscilloscope and also a frequency analyzer. It was found through 
testing that the SRK when in operation only utilizes approximately 2.7 milliseconds total 
in handling all the associated routines. For further information on this counter/timer see 


reference 21. 


2. Motion Control 


Referring back to Figure 4.4, motion control encompasses several major parts 
which are woven into a tightly controlled structure for driving and Steering Shepherd’s 
wheels. The bodyMotion function takes as input the mode of motion desired from the 
user. Using this mode and the necessary instructions programmed, it provides 5 inputs to 
the wheelMotion function which are: motion, speedDot, thetaDot, omegaDot, and 
vehicle. Motion is a structure consisting of the user’s input of speed, theta (direction of 
travel of the vehicle), and omega (rotational speed of the vehicle). Vehicle is a structure 
consisting of the x and y coordinate of the vehicle on some x-y plane for tracking 
purposes and the heading of the vehicle. SpeedDot, thetaDot, and omegaDot all are a 
derivation of speed, theta, and omega over time. 

The wheelMotion function takes the given inputs along with inputs from a 
feedback loop for the actual positions of the encoders for both driving and steering of the 
vehicle, and performs calculations based on the theory discussed in chapter VI. The 


results of these calculations are then sent to the driveMotors function which provide these 
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values to the servo drivers for steering and driving the vehicle wheels resulting in the 


motion of Shepherd. 


Se User Interface 


The user interface is facilitated through the use of a Texas Instruments laptop 
computer running the Windows 95 operating system. By utilizing the HyperTerminal 
program accessory to connect the laptop to the unix system via an ethernet connection, 
program files can easily be receive from the unix system after being compiled and the 
executable files sent to the taurus board. After successful downloading of the program to 
the Taurus board, the user can then control the operation of Shepherd through the laptop 
keyboard. Figure 4.5 illustrates this user interface after the program is successfully 
downloaded and ran. An on-screen menu is displayed with a description of Shepherd 
motion that can be initiated via keyboard strokes. 

This menu is located in the file user.c (Appendix C). It is a simple character 
definition which is called from the user.c file. The menu choice inputs from the keyboard 
are converted to ascii characters which are interpreted in the user() routine switch 
statement to launch the associated menu item seen in figure 4.5 on the next page. 

Also, part of the user interface is a joystick. This joystick interface is activated 
when menu choices 3 and 4 are selected. It controls Shepherd’s wheels for both steering 


and driving. This will be discussed in Chapter VI. 
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Figure 4.5: Display of shepherd menu for user interface. 
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V. EXPERIMENTAL RESULTS ON MOTION CONTROL 


A. OVERVIEW 


Chapters III and IV of this thesis discuss how each servomotor can be accessed 
and voltages applied. However, the aforementioned chapters do not explain how the 
incremental inputs placed on the DA translate to wheel speed or what the maximum and 
minimum wheel speeds are. So, how were the maximum and minimum wheel speeds 
determined? Moreover, what type “controls” are required to ensure that each wheel has 
the same driving velocity or angular velocity? We used a modified version of the 


“scientific method” to establish and carry out the experiments for the Shepherd vehicle. 


Scientific Method and Approach 





Validated 
Hypothesis 







General 
Assumption (s) 


Test and 


Modify or Change 
Hypothesis 


Figure 5.1: Modified version of the scientific method and approach used for Shepherd development. 
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As with any experiment, results must be consistent and reproducible. Considering 
the vehicle as a “closed system” observation of all experimental results should lead to 
deduction, connection, or correlation. As is common with the scientific method progress 
can be very rapid or slow. Sometimes there were unexpected results (e.g., singularities), 
and in some cases our dead-end path (failed experiment segment) provided some very 
important insights which helped to improve vehicle performance. An Ockham’s razor 
approach was taken when making investigations and deductions—meaning that any 
unknown phenomena or behavior should be explained in terms of what is already known 


(and testing the simplest possibility first). 


B. WHEEL DRIVING 


Once an understanding of the mechanics and providing the coded structures to 
move the wheels had been achieved the concept of precise and controlled driving motion 
becomes the focus. The basic initial idea was to measure the counts from the counter 
board, measure wheel revolutions over time. Again reiterating a modified scientific 
method was used— and apriori data such as gear ratio and other engineering data were 


used to verify results. 


1. Developing Driving Constants 


The digitloRadDrive constant [Appendix J, Consolidated header files, line 364] 
was the first constant to be determined for wheel driving. The biggest problem with 
developing this constant was working around the 10-millisecond timer interrupt and the 
lack of a fully functional operating system. Since Shepherd’s SRK was developed from 
the ground up within the last year it has limited I/O capabilities, and there is no long-term 
storage on the Taurus board. What this means is that the results of test programs would 
have to be sent to a monitor (VT220) or to a printer. We decided to print to both the 
monitor and printer. However, this also yielded unexpected results. The print function 
used too much of the 10 milliseconds to allow for proper functioning of the process 


currently running. Hence, we moved the print function from the real-time portion of the 
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code to alleviate this problem. Moreover, this became a useful technique that was used 
throughout Shepherd development and testing. 

The first step in determining the number of counts is reading the counter board. 
In the algorithm used in the SRK, the initial values on the counter board are read, stored, 
and read again after a complete revolution of the tired is complete; the absolute difference 
between the initial counter value and the final counter is the actual count take for the 
revolution. It should noted however that consecutive reads of the counters is actually 
accomplished and the values adjusted for the transition from Oxffffff to Ox000000 and 
vice versa (24 bits) [Appendix E, Motor.c, lines 607-621]. An example of the code to 


read the encoders for driving can be seen below: 


void readDriveEncoders(unsigned long int array{]) 


t 
unsigned char *p=(unsigned char*) VMECTRI, cl, c2, c3; 
fae xe 


long int temp; 


for (ix=0; ix<4; ixt++) { /* read all four motors subsequentially */ 


*(p+3)=0x03; /* load output latch from counter */ 
*(p+3)=0x01; /* control register, initialize two-bit 
output latch */ 


/* read three bytes for specific counter 1x and save in status */ 
/* first access to Output Latch Register reads least significant */ 
/* byte first 2) 


cl = *(pt+1) & Ox00ff; 

c2 = *(pt1) & Ox00ff; 

c3 = *(pt+1) & Ox00ff; 

array(ix] = ((unsigned int)c1)| ((unsigned int)c2 << 8) | 
((unsigned int)c3 << 16); 

p=pt4, /* increment pointer for next counter */ 


return; 


ao 


\ /* end of readDriveEncoders */ 


Secondly, a method to display the enconder data is required. The SioOut 
[Appendix E:, Motor.c, lines 483-490 and 381-399] display routine from SRK was used 
for this, but failed because the time required to display the data exceeded 10 milliseconds. 
So, we moved the display “call” for the function from the real-time portion of the code. 
In SRK, immediately following the “driver” routine’s call the following type routine 


would be placed in the main “user” routine: 


while(1) 
{ 
while(edCounter%200 != 0) {}; 
displayCount(); 
i" 


The aforementioned routine would print based on the edCounter and the timer. The 
counter being incremented every 10 milliseconds. Hence the value is displayed every 2 
seconds in real-time. The key is the fact that the printing is being executed outside of the 
driver routine. 

Now all the elements are in place to determine the digitToRadDrive constant. 
After testing each wheel for a minimum of 1000 revolutions (called double pi or DPI 


below) the digitToRadDrive constant was determined: 


Hdefine digitToRadDrive -6.015495746e-5 
/* driving constant rad/count = DPI/104450 May 8 */ 


/* Experimental Results by Ed Mays May ja 
/* Wheel 1 count = 104456 “ie 
/* Wheel 2 count = 104435 ty 
/* Wheel 3 count = 104454 a, 
/* Wheel 4 count = 104455 ay 
/* Average count = 104450 a) 
/* cf. 2048 * 51 = 104448 “4 
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Verification of the constant was made possible by the engineering data. Given the range 
of inputs (same as Figure 3.12) to the Servomotors and the gear ratio (section 3.B.3) the 
digitToRadDrive was be verified above (cf. 2048 * 51 = 104448). The value of 51 vice 
50 for the gear ratio (was determined empirically to be 1:51). 

The digitfoCmDrive was much more trivial to determine. The digitToRadDrive 
provides the number of counts, knowing the wheel radius (18.9cm), and using circle 


circumference formula yields: 


#define digitToCmDrive 0.0011369287 
/* driving constant cm/count = digitToRadDrive*18.9cm Ed Mays */ 
Pet - 


Now having the “drive” encoder count allows the computation of distance traveled (cm) 
by the vehicle; and coupled with the timer interrupt allows for the computation of 


velocities and accelerations. 
es Measuring Wheel Speeds 


The next step in control of the wheels is being able to manipulate wheel speed. 
Developing wheel speed control was one of the projects many dead-end path’s—that 
eventually lead to great results. There were three problems with the work presented here. 
First, the inputs or digits were not applied at the lowest (hardware) level using 
SpeedDigits [Appendix E], instead the value was considered desiredSpeeds. Secondly, 
the input values were massaged before being used to ensure that all the system hardware 
would accept the range of values. Thirdly, the uniqueness of each driving motor and 
natural output variance over a range of inputs was not considered (e.g., used averages 
instead of individual motor data). How did this happen? A minor communication error 
and poor naming of arrays made this possible. However, a look at the results and the 
logic for deriving them will provide insight towards the actual solution. 

Include are an estimated velocity and the software-measured (actual) velocity as 
determined by software. The estimated velocity was determined by applying the 
requisite input and measuring the number of seconds it took for wheel one to complete 10 
revolutions (e.g., wheel radius 18.9cm, distance traveled = 2 * PI * Revolutions * radius 


=1187.5220 cm); inaccuracies from this measurement came from hand timing and the 
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version of pi used on a desk calculator. Velocity calculated from V=distance (cm)/time 
(seconds). 

The software-measured velocity used the digitfoCmDrive constant, which has the 
unit’s cm/count. The algorithm subtracts the previous count from the present counter (as 
read from the counter board) and multiplies the result by digitToCmDrive leaving the 
outcome with the unit of cm. This outcome is then divided by .01 (DeltaT) representing 
100th of a second or 10 milliseconds (corresponding to the system interrupt). The 
99999999 represents a value not representable by the counter board. The output 
displayed to the monitor every 100 calls of the routine (mod 100). An example of the 


code to compute each wheel’s driving speed can be seen below: 


void computeActualRates() 
{ 
int 1; 


double count,speed; 


for(i=0; i<=3; i++) 
{ 
if(PreviousCountSpeed|{i] == 99999999) /* for derivative for speed */ 
Drive_Speed_Actuallfi]= 0.0; 
else 
Drive_Speed_Actual{i]= 
(convertDifference((WheelDriveValues[i] - PreviousCountSpeed{i])) 
*DigitloCmDrive[i])/DeltaT; 
PreviousCountSpeed{i] = WheelDrive Values{i]; 
j 
j 
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Estimated Velocity Software 


(cm/s) 


Time 
Measured Velocity 


(cm/s, average) 


Stop watch 


(sec) 
0 | te.27see | 0.21349 | 0.23235 
50 | Nodata | ——Nodata | 50.0702 
60 || Nodata | ~——Nodata | 6.93937 
| 80 | Nodata | Nodata | 8.29040 
| 90 | No data | Nodata—|_——87.65720 


Figure 5.2: Inputs and results from massaged data (error). No data entries exist because the 
revolutions were too fast for hand timing. 

If an input of 100 is used the estimated values for velocity are nolonger linear. If the 
inputs vs. velocities were linear the ratio 20/20.42170=100/X used to predict the 


estimated velocity would yield a velocity of 102.32355 cm/s; however, the software- 


measured velocity was 87.54350 on average for all input values from 100 to 1000 (Figure 
a.) 
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Figure 5.3: Inputs vs. measured velocity. Figure showing a velocity saturation. 








Hence, the relationship between the input and the velocity look somewhat linear until the 
area where the input is greater than 70-- after this velocity saturation Seems to occur (or 


maximums reached). Moreover, if one closely looks at the slope (i.e., y = mx + b) 
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between the input ranges 20-30, 83-84, and 87-88 it is clear that the slope changes 
dramatically over these regions. Even though the wrong input structure and mechanism 
was used to generate this data a lot was learned. For instance, motor performance is not 
completely linear and each motor has somewhat unique characteristics. Once the 
aforementioned problems were identified and corrected the correct data [Appendix N] 


could be plotted. Figure 5.4 1s a plot of the correct data. 


Driving Velocity vs. Input 
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Input Values (units) 
Figure 5.4: Driving Input vs. Velocity plot. Input units or digits are written directly to SpeedDigits. 
The slope changes for the overall graph are constant, making it look linear. However, over some 


regions this graph is non-linear, this will be dealt with under the controls section. Velocities also 
independently verified by use of tachometer. 


3: System Controls 


So far controlling the speed of individual servomotors has been discussed. 


However, it is known individual servomotors provide differing outputs for the same input 
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ranges in some cases. Moreover the wheels must be coordinated and work together. The 
goal of this project was to have an actual driving speed that has less than | % error when 
compared to the desired input speed (for each wheel). How is this close tolerance 
accomplished? This small margin of error is accomplished by using well-established 
concepts from “control systems” theory. A general control-system structure contains 
inputs (or reference commands), a controller (with external power), control forces, a 
controlled system (plant), disturbance inputs, outputs, and output monitoring. Control 
systems are almost a discipline unto themselves requiring knowledge of differential 
equations and Laplace transforms. Shepherd is looked at as a closed-loop where output 
monitoring is accomplished through sensors (encoders) and the information passed 
through feedback channels. The feedback results in a closed loop signal or information 
flow. The controller design for Shepherd is linear and considered a single-input-single- 
output (SISO) system. Hence, conceivably the state-variable or the transfer-function 
(input-output) method could be used here. What technique did we use? We used the trial 
and error technique [Ref. 17]. The trial and error technique (Figure 5.5) was chosen 
because of the skill and knowledge levels of the Shepherd team. The advantages of this 


technique are: 


Simple mathematical tools are used 
Vast amounts of experience have accumulated 


Especially well adapted for use with computers 


eo 


Linear designs usually are acceptable 
The disadvantages are: 
1. Inconsistent performance specifications (PS) can be encountered 


2. Design is not optimal 
3. The method is usually suitable mostly for SISO systems. 
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Figure 5.5: A flow graph of the trial-and-error design process [Ref. 18]. 


Using the experience of both Professors Kanayama and Yun as the guide the 
“black box” servo structure was developed (Figure 5.6). The servo structure is called 
“black box” because of the lack of understanding of the servomotors at that time 
(however, inputs and outputs could be measured). The previously mentioned PS was an 
error rate of less that 1% of the given reference input. There were other PS’s governed 
by heuristics. For instance, it was desired that the actual output velocity converges (in 


the mathematical sense) on the reference or input velocity, with near perfect static follow- 


up. 
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Figure 5.6: “black box” servo structure with feedforward and feedback compensation. 


Vibrations and other unacceptable behaviors were used to determine if a smooth 
and acceptable convergence had been achieved for the gain used (e.g., if the robot was 
shaking, this was not acceptable). An acceptable gain would be one that produces an 
oscillatory response that converged quickly to the reference input (Figure 5.7). Again 
this same type information could have been determined by a better scientific guess [Ref. 


19] using a closed-loop differential equation. 
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Figure 5.7: Proposed “‘proper” oscillatory response as a function of loop gain. Response determined 
for Shepherd using experience and heuristics. 
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Previously, it was stated that the input vs. velocity plot was not “truly” linear. 
These non-linear ranges have severe physical realities. Also, mentioned was the point 
that each servomotor produces an output that may not be the exact same as the outputs of 
the other system servomotors given the same input. And the stated PS requires that 
servomotor outputs be within 1% of the desired input or input speed. This translates into 
several challenges. First, using the feedforward technique constants were developed to 
ensure that the PS of 1% is met. The following algorithm or averaging technique was 


used for direct testing of inputs: 


if (v} <V< vp) 
d = d; + (dz — dy)/(v2 —-v1) * (Vv — v1) 


The v’s and d’s above are the same as the velocities and inputs in figure 5.8. 





Figure 5.8: Illustration of averaging technique used to select “‘good”’ 
input ranges. 


Using the aforementioned averaging technique the constants were developed that 


maintained all servomotor outputs within 1% PS. 
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These constants are called K1-K4, and K6. These constants represent the 
feedforward values that are applied to each servomotor over a specified range of desired 
speeds. The desired speed ranges give are because of the piecewise continuity chosen 
because of the non-linearity’s in the input vs. velocity plot for the servomotors. The “K”’ 
constants were developed with no load on the system (e.g., wheels free floating). Listed 


below are the constants K1-K4, and K6 (speed is in centimeters/second): 


K1[0]=11.448;/*0<=speed<=5,*/ 
K1[1]=11.500; 
K1[2]=11.496; 
K1[3]=12.375; 


K2[0]=11.500;/*5>speed<8*/ 
K2[1]=11.500; 
K2[2]=11.644; 
K2[3]=12.000; 


K3[0]=11.611;/*8>=speed<20*/ 
K3[1]=11.585; 
K3[2]=11.686; 
K3[3]=11.840; 


K4[0]=11.711;/*20>=speed<=70*/ 
K4[1]=11.659; 
K4{2]=11.705: 
K4[3]}=11.727: 


K6[0]=11.710;/*70>speed<K5*/ 
K6[1]=11.700: 
K6{2]=11.700; 
K6[3]}=11.715: 


Above the bracketed values are array element numbers. Element [0] refers to 


wheel 1, motor M1; element [1] refers to wheel 2, motor M2; element [2] refers to wheel 
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3, motor M3; and element [3] refers to wheel 4, motor M4. A constant K5 was also 
defined as 87.4 cm/sec, and used a safety cutoff for the maximum speed. Figure 5.9 
shows the values for a few selected desired or commanded speeds (most are within the 


require 1% error 1n PS). 


Desired Speed M1 Speed M2 Speed M3 Speed M4 Speed 
yo | 10.20 10.08 10.10 9.89 


















20 20.12 20.11 20.11 19.93 


vag tee: 
40 | 3998 | so. | 39.98 | 39.98 
oo | won| 59.77 | woe] wn 


Figure 5.9: Desired (commanded) speeds vs. actual “free floating” motor speed. The application of 
feedback is expected to move the speed of M1 into the required 1% error for the PS. 





At this point all the tools and techniques (1.e., trial-and-error, scientific method, 
and the experience of Professor’s Kanayama and Yun) are in place for the application of 
system controls. How are the system controls (Figure 5.6) implemented? _ First, the 


algorithm is presented: 


SpeedDigit = velocityReferenceTable(Omega_Speed,ix) + 
DriveFeedBackGain*(Omega_Speed - Drive_Speed_Actual|ix]); 


SpeedDigit represents the actual digit value being applied to the servomotor. In the 
feedforward part of the loop (Figure 5.6) the commanded velocity is multiplied with a 


é¢ 


constant A/, which corresponds to the “kK” constants described earlier in this section? In 
the code segment above the function ‘“‘velocityReferenceTable” is called (prior to the 
addition of DriveFeedBackGain*(Omega_Speed - Drive_Speed_Actual[ix]) ). The 
“velocityReferenceTable” applies the proper “K” constant for the range the commanded 


speed (Omega_Speed) falls within. 
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A detailed look at velocityReferenceTable is provided below: 
double velocityReferenceTable(double desiredVelocity,int 1) 


{ 
double inVelocity, 


out Velocity; 
inVelocity=new_abs(desiredVelocity); 


if (inVelocity>=0.0 && inVelocity<=5.0) 
outVelocity = inVelocity*K1 [1]; 


if (inVelocity>5.0 && inVelocity< 8.0) 
outVelocity = inVelocity*K2[i]; 


if GnVelocity>=8.0 && inVelocity<20.0) 
outVelocity = inVelocity*K3[i]; 


if (inVelocity>=20.0 && inVelocity<= 70.0) 
outVelocity = inVelocity*K4]1]; 


if GnVelocity>70.0 && inVelocity<K5) 
outVelocity = inVelocity*K6[i]; 


if (inVelocity> K5) 
out Velocity=1023; 


if (desiredVelocity< 0.0) 


outVelocity = - outVelocity; 


return out Velocity; 


} /* end velocityLookupTable */ 
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It should be noted that if the inVelocity is greater than K5, then the outVelocity is given a 
value of 1023—this ensures there are no system resets because the input digits are too 
large. All other velocities are multiplied by a specific “K” and the value returned. In the 
feedback part of the loop (Figure 5.6) the difference between the commanded velocity 
and the actual velocity is multiplied with a constant DriveFeedBackGain (also K2 in 
Figure 5.6) [Appendix E, lines 46 & 47). Again this 
(DriveFeedBackGain*(Omega_Speed - Drive_Speed_Actual[ix]) ) is added back into the 
inputs used for the next time the process is run (based on the 10 millisecond timer 
interrupt). 

Hence, now the final key to this control system would be finding a 
DriveFeedBackGain constant that would provide the desired “proper” oscillatory 
response (Figure 5.7) and ensure the servomotor output velocity is within the 1% error of 
the commanded velocity required by the PS. Using Occam’s Razor, the trial-and-error 
flow diagram (Figure 5.5), the iterative approach in the scientific method (Figure 5.1), 
and the heuristics provided by Professor’s Kanayama and Yun, the search for the best 
DriveFeedBackGain was initiated. The heuristics used were observation based. First, 
the “gain” used could not cause the vehicle to shake in any visible manner. Secondly, the 
“gain” used had to quickly move the actual servomotor speed to the commanded speed if 
there was a difference. Pseudo random values were chosen as gains, based on the 
experience of the aforementioned professors. The real number range [-1.0, 1.0] was used 
to test the gains. On the negative end of the range the gain was incremented by +.05, 
until the gain equaled zero—the results were not acceptable. On the positive end of the 
range the gain was decremented by +.05, until the gain equaled zero—at .8 the gain 
showed the best results (i.e., range [0.0, 1.0]). The gain was defined as 
DriveFeedBackGain = .& [Appendix J, Consolidated header files, line 389]. Hence the 
gain met all the criteria for an acceptable design (Figure 5.5) and validates the ‘black 
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box” servo structure as envision by professors Kanayama and Yun (Figure 5.6). 
Moreover, the experiment can be considered a multidisciplinary success between physics, 


electrical engineering, and computer science. 
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C. WHEEL STEERING 


As with wheel driving, the same approach to wheel steering was used in 
measuring the counts from the counter board and measuring the rate of turning of each 
wheel over time. The goal was to observe and measure each wheels turn rate and create a 


feedback compensation loop as in wheel driving to ensure that the PS of 1% is met. 


1. Developing Steering Constants 


The steering constants were developed in the same way as the driving constants. 
The counter board was read for steering values and then displayed . A digitToRadSteer 
(input digits per radians for steering) constant value of —6.817692391e-5 (rad/count 
(2*pi)/(2048*45)) and RadRateTodigit constant value of 195.4155 (digits/rad/sec 
1023/5.23598) was determined by observation of the data forthcoming. 


2: Measuring Wheel Rate of Turn 


In measuring the wheel rate of turn, the same approach was taken as discussed 
before. An estimated rate of turn and the software-measured (actual) rate of turn are 
included. The estimated rate was also determined by applying an input and measuring 


the number of seconds it took for a wheel to completely rotate 360 degrees. As can be 


deduced, a certain amount of error was introduced due to human timing interaction. 
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Desired rate of Time Estimated Rate Software 
Stop watch (rad/s) Measured Rate 
(rad/s, average) 


|_10_|_Nedata_|_Nedata_ 5.23598 


No data 5.23598 
30 5.23598 


Figure 5.10: Inputs and results from massaged data (error). No data entries exist because the 
revolutions were too fast for hand timing. 





By observing the data from the above figure we can confirm that a certain amount 
of error is inherent in the system. As the input value increases from 0 to 5.5 radians per 
second , a linear correspondence tended to exist. However, after an input of 5.5 rad/s the 
software measured average tended to be 5.23598 rad/s resulting in a saturation state. This 
result in itself could not give us the exact range of values where this occurred. Therefore, 
manipulation of the steerDigits was necessary. Appendix O contains these results given 
in inputs of digits vice desired rate of turn. At the maximum input speedDigit of 1023 
Saturation is reached. The wheels will only turn at an average rate of 5.235 radians per 


second. Thus the software measured average of 5.23598 rad/s was adopted. 


a Steering Feedback 


By further observation of the data obtained, an average was used in obtaining the 
rate of turn. Not all the wheels turn at the same rate. The objective was to have a less 
than 1% PS for optimization. To achieve that another “black box” servo structure was 
developed . The Figure 5.11 is a pictorial representation of this and is a little different 


than that of wheel driving. 
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desired rate 


(+) Shaft Encoder 
desired position (position) 
O->= Laan 


positional error 


actual rate ise 


actual position 


Figure 5.11: “black box” servo structure with series and feedback compensation for wheel steering 
feedback control. 


In finding the K constants from the figure, trial and error is also used. The 


representation of the algorithm 1n code Is presented: 


Steer_Digit = rateReferenceTable(desiredAngleRates[1x}) 
+ steerFeedbackGain* (desiredAngleRates[ix]-actualAngleRates|1x]) 
+ angleFeedbackGain*norm(desiredAngles|ix]-actualAngles[ix]); 


Steer_Digit represents the actual digit value being applied to the servomotor. The 
rateReferenceTable function simply converts the inputed rate to digits or, in the case 
where the inputed rate is larger than 5.235, clips it to within limits. The function is given 


below: 


double rateReferenceTable(double desiredRate) 
{ 


double inRate, 
outDigit; 


inRate=new_abs(desiredRate); 
if (inRate<= 5.234) 
outDigit = inRate*195.4155 ; 


else 


outDigit=1023; 


5] 


if (destredRate< 0.0) 
outDigit = - outDigit; 


return outDigit; 


The two K constants steerFeedbackGain and angleFeedbackGain were determined 
by trial and error. AngleFeedbackGain was first maintained at 0.0 while testing 
steerFeedbackGain. SteerFeedbackGain was made very low at the outset and increased 
through each test until the vehicle displayed unusual behavior such as shaking while 
Operating its steering function. At this point the value was lowered and then the value for 
angleFeedbackGain was increased in the same way. These values became optimal at 


100.0 and 1000.0 for steerFeedbackGain and angleFeedbackGain respectively. 


4. Wheel Testing 


It was discovered while operating the vehicle that wheel 4 would on occasion be 
very badly misaligned from the other 3 wheels. Even after repeated realignment it would 
not operate as the others. At suspect was the thought this problem was software related . 
To test this, a routine was inserted into the SRK driveMotors() function with a menu item 
on the user interface. This routine simply turned wheel 4 360 degrees in one direction 
until the wheel aligned read the encoders for angle position, paused one second and then 
turned it in the opposite direction. At each pause, the wheel position was displayed to the 
interface screen and recorded. The data obtained is presented below in Figure 5.12 for 10 


iterations for clockwise and counterclockwise rotation: 


| Clockwise Rotation __|_Counterclockwise Rotation _ 
000.867 360.390 

4 | 933 | 60. 
6 | 99360308 
| 












000.992 





ye 






Clockwise Rotation Counterclockwise Rotation 


ass | 000.902 360.394 
Pe) oe! 000.996 360.445 


000.996 360.476 


Figure 5.12: Wheel 4 data based on position of rest after direction of turn. 






The average values for rotation in both the clockwise and counter clockwise directions 
were 000.955 and 360.426. These were averaged for 20 iterations see Appendix O for 
the full data set. Even though this data was obtained while the wheel was in a free 
floating environment without added friction, it still proved that wheel 4 was operating 


within 1% PS. Therefore, from this result a conclusion is drawn that the problem is not 
software related in nature but maybe mechanical. 
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VI. MOTION MODES 


A. OVERVIEW 


Chapter II mentioned several modes of motions that can be exhibited by 
Shepherd. Due to the nature of Shepherd’s characteristics of having four wheels that can 
be independently operated with two degrees of freedom, this makes it possible to obtain 
three degrees of freedom motion. In this chapter we will discuss the modes of “Tornado” 
(complex motion), Joystick controlled motion and searching motion simulation. The 
other motion modes are encompassed by these motions. The code for the other motions 
are provided in the Appendices. Also, the Tangential mode will not be discussed but will 
be left for a future reasearch topic and implementation. However the ground work as 


well as the code is in place for implementation (Appendix D). 


Before embarking on this discussion on motion modes the theory or basis for 
motion control must be presented This control motion theory, as well as the figures to 
follow, was taken from the works of Professors Yutaka Kanayama and Xiaoping Yun 
{refs 23 & 24]. First a vehicle coordinate system is defined on a ngid body robot. A 


configuration q 1s defined as 


(p, yW) = (x, y), y), 


where p is the positioning of the vehicle origin and psi is the heading orientation of the 
vehicle Xv-axis. Next in describing the motion of the vehicle’s configuration which is a 


function of time, the following is the definition: 
q(t) = (p(t), w(t)) = ((x(b), y(t), wot), 


where p(t) 1s the translational component and y(t) is the rotational component of the 
vehicle motion. Figure 6.] is an illustration of this configuration and motion. Because 
the vehicle possesses 2 dimensional positioning it can exhibit 3 degrees of freedom 


motion. This motion contains three variables of : 


e Translation speed -- v(t) = V ((dx(t)/dt)” + (dy(t)/dt)°), 
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e Motion direction -- 6(t) = atan2(dy(t)/dt, dx(t)/dt), if v(t) > 0, 
e Rotational speed -- w(t) = dy(t)/dt. 


Resulting in a motion description of: 


Qit) = (v(t), O(t), @(O)). 


(D) 


Configuration 





(IT) 


Motion 





Figure 6.1: Configuration and Motion of a Rigid Body [Ref. 23]. 


As was mentioned in Chapter II, 3 typical vehicle motions are possible. The 3 
typical motions are illustrated in Figure 6.2. Part (a) represents an all too familiar motion 
exhibited by a normal automobile or bicycle and is referred to as tangential motion. This 
is characterized by the fact that the vehicles heading orientation is equal to its 
translational motion direction ( dy(t)/dt = @(t)/d(t)). Part (b) depicts a motion called 
constant orientation where the vehicles heading orientation 1s constant or rotational speed 


of the vehicle is 0. And part (c) shows a complex motion which incorporates rotation 


2) 


(a) p(t) 


(b) p(t) 


(c) p(t) 


Figure 6.2: Typical vehicle motions [Ref. 23]. 


independently superimposed on a translational motion. This complex motion is the basis 


for the ““Tornado” motion mode to be discussed. 


B. “TORNADO” MOTION 


In analyzing this motion mode, several more discriptions will have to be made 


clear. One is that of a point. A point is defined as 
Pi = (X1, y1) e (0,0) 


described in the vehicle coordinate system. On the rotary vehicle it corresponds to a 
wheel . In the case of wheels 1-4, it would be (40,-40), (40,40), (-40, -40), and (-40,40). 
So we will have to evaluate how these wheels move while the vehicle 1s executing the 


input motion or Q. In order to evaluate this, the polar coordinate representation 1s 


another description that is needed. This representation is defined as (Pp, @) and is 


represented as 
i= Vix? +y1) and a = atan2(yi, X}). 


The subscript is a representation of the wheel number and can represent any wheel based 
on the wheel location in the vehicle coordinate system. Figure 6.3 is a representation of 


the composite motion of a point on a vehicle. 


e 





Figure 6.3: Composite Motion of a Point on a Vehicle [Ref. 23]. 
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Given the above, a current configuration and motion of 


q(t) = ((x(t), y(t)), wt), 
Q(t) = (v(t), B(), w(t), 


in the global coordinate system, the x and y-components of vj,(t) and v,,(t) in the global 
coordinate system can be determined mathematically [Ref. 23 p.3]. The motion speed 
v,(t) and direction 6;(t) in the global coordinate system, motion direction 6,°(t) in the 


vehicle coordinate system, and rotation rate @,. of p1 or any wheel is: 


vi(t) = VC (vix(t)” + (viy())”) 

6)(t) = atan2(vj y(t), vix(t)) 

Q,”(t) = O1(t) - w(t) 

= ((v’ 8 +p" )+vpa(wt J)sin(O-y O)+P(v9- @)cos((O-w-a))/v," -@ 


0 is theta dot, ° is omega dot, and ** is motion speed dot. 


Corresponding to the above equations in SRK is the following code from Appendix D 


(movement.c): 


ro=whp{i].rho; 

ro2=rorro; 

beta=vehicle.heading+ whp[i].alpha; 

vlx = speed*cos(theta)-(whp[i].rho*omega*sin(beta)); 
vly = speed*sin(theta) + (whp[i].cho*omega*cos(beta)); 
desiredSpeeds[i] = new_sart(v1x*v1x + vly*vly); 


if (new_abs(desiredSpeeds[i]) > 0.01) { 
desiredAngles[i] = atan2(vly,v1x) - vehicle.heading; 
wheelAngleV = motion.Theta - vehicle.heading - whp[i].alpha; 
desiredAngleRates[i] = ( (speed*speed*thetaDot + ro2*Omega3) 
+speed*ro*omega*(omegatthetaDot)*sin(wheelAngleV) 
+ro*(omegaDot* speed-omega*speedDot)*cos(wheelAngieV) ) 
/( desiredSpeeds[i]* desiredSpeeds[i]) - omega; 


58 


desiredAnglesOfi] = desiredAngles|j]; 
desiredAngleRatesO[i] = desiredAngleRates|}]; 
j 


The above code is very straight forward. A direct correlation can be discerned 
from the theory to the implementation. The variables were named as closely as possible 
to match the theory presented. The resulting code provided the mathematical 
computation providing the resulting values from a rotation superimposed independently 
on a translation motion. It is not shown, but all variables with brakets enclosing an ‘i’ 
represent a wheel. The entire routine is enclosed in a for loop which is iterated four 
times. Therefore, a resulting value is computed and provide to each wheel’s servo for 
both driving and steering . 

A detailed proof of the rotation rate of the moving direction at a point can be 
found in Ref. 24. As was stated earlier, this motion control theory is from the work of 
Professors Kanayama and Yun. The authors simply implemented this theory in code and 


applied it to the rotary vehicle. 


Cc. JOYSTICK CONTROLLED MOTION 


As discussed in chapter IV the user interface is provided by a laptop that includes 
a selection menu of shepherd functions. There are many options (still expanding, a work 
in progress) on the menu. The two of concern here are options three (3) and four (4), 
Straight motion by joystick and XY-motion by joystick. Actually the emphasis will be on 
option four because option three can be considered a logical subset of option four. 

The “driver” function discussed in the file movement.c 1s called every 10 
milliseconds. One of the key functions executed under driver is the call to another 
function named “bodyMotion”. The bodyMotion function is also located in movement.c. 
When a user selects option three or four from the menu a motion mode 3 or 4 is chosen. 
If the user chooses option four the motion mode is 4. The case the user chose causes the 
joystick to be read by the readJoyStick function (Appendix H, utils.c). This function 
reads the three ports (A, B and C) from the Intel 85C55 Parallel Port 1 (Taurus board) 


and converts them into an ASCII string (code segment below): 


Do 


void readJoySuck(void) 

{ 
unsigned int 1,index; 
unsigned char *ctrlPort=(unsigned char*)PIO1_CTRL; 
unsigned char *dataPort=(unsigned char*)PIO1_DATA; 
unsigned int pioPort! [3]; 
double a= 0.1, xx, yy, zz; 


*ctrlPort=0x9b; /* set all ports (A,B,C) into input mode (read only) */ 
index=10; /* position for x-digits in string JOYSTICK 7 


for (1=0;1<3;1+ +) 


pioPort1 [i] = *(dataPort+1); 


xx = (double)pioPort! [0]-128.0; 
yy = (double)pioPort1 [1]-128.0; 
if (xx >= 0.0) 


xx = xx*xx/100; 


else 

SX eo) 
if (yy >= 0.0) 

VY = yy yp 00; 
else 

yy = -yy*yy/100; 


joyStick.x = a*(xx) + (1.0-a)*joySuck.x; 
joySuck.y = a*(yy) + (1.0-a)*joySuck.y; 


if (pioPort1[2]==0x03) 


setVME((unsigned char *)VME9210,0x00); /* no button pressed = */ 


else 4 
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setVME((unsigned char *) VME9210,0x02); /* if any button pressed */ 


j 
} 


It should be noted that the joystick input integer range is from [-127,128]; the 
intersection of the *x’ and ‘y’ axis on the physical joystick defines the center (x=0, y=0). 
Once the port is read some data smoothing 1s done. Due to the sensitive nature of the 
inputs a parabolic function was added for control (this can be seen above with the 
manipulation of the xx and yy variable). The purpose of the parabolic function is to 
ensure that when the joystick input values are small (near zero, center on the physical 
joystick) the slope changes will be of minimal effect, however if the input values are 
large (away from the physical joystick center) the effect on velocity or steering will also 
be proportionally large. The smoothing is continued for because of the possibility of very 
quick slope changes in the data being read-in. The objects joyStick.x and joyStick.y 
receive values that are only 10 percent (a = 0.1 in the code segment) of the xx or yy value 
plus 90 percent of the previous value for xx or yy. The aforementioned smoothing 
techniques were developed based on the experiences of Professor Kanayama an the 
constant “‘a = 0.1” determined by testing for the “best” hand feel and response. 

Upon completion of the read and smoothing of the joystick data, these values (i.e., 
speed and theta) are passed to the wheelMotion function described in the Shepherd 


Motion Control Architecture (see code segment below): 


case 4: /* X-Y Motion by Joystick */ 
readJoyStuck(; 7 emo july, 
speed = -joyStick.y*0.1; | /* speed control, 0.1 determined by testing */ 
theta = -joySuck.x*0.02; /* steering control, 0.02 determined by testing */ 
NelGgeaer == nll tenoch— Sle 
if (theta < -HPI) theta = -HPI; 
/* omega = -joySuck.omega*0.1;*/ /* pending ejym 24 july 97 */ 
break; 


6] 


The two “if” a statements with the theta conditions above reflect the capability of 
the rotary vehicle to complete perpendicular driving and parking. Actually, the theta 
values (steering angle) of the Shepherd vehicle are unlimited, however they are 


constrained here for ease of use and control. 


D. SEARCHING MOTION 


The searching motion discussed here is based on the requirement to have a 
smooth technique that allows shepherd to evenly and precisely search an area for UXOs. 
The aforementioned search algorithm and its implementation are not trivial. The 
algorithm and simulation presented here are the results of a lifetime of work by Professor 
Kanayama. Professor Kanayama’s expertise in the areas of motion planning, motion 
design, vehicle kinematics, sensing, guidance, learning, environmental representation, 
and control architectures for autonomous vehicles was the major influence. Professor 
Kanayama’s work on the Yamabico-11 robot includes development of composite 
function, line tracking, circle tracking, and neutral switching technique [Ref. 7]. For the 
aforementioned search algorithm and simulation the composite function and line tracking 
technique will be used. 

The goal of this simulation is to show that if given an orientation for the vehicle 
body and a given path, that the path can be tracked smoothly and the vehicle orientation 
will also change to ensure area coverage of the path traveled. Why is this important? 
This is critical because the desire is for the vehicle to search the path in the most safe, 
smooth, and efficient manner. The first assumption is that the time required to move 
across a path is 10 milliseconds or 0.01 seconds. Secondly, an assumption can be made 
for the vehicle orientation (called psi here), psi starts at 3*PI/4.0. Along the path traveled 
the orientation or psi will move from 3*PI/4.0 to PIJ/4.0 (having a net change of PI/2.0). 
From the aforementioned change in psi, the incremental change over time can be 
determined (this incremental change is called omega). Dividing the net difference in psi 


(Pi/2.0) derived omega by 10 milliseconds, resulting in an omega value of 0.1570796327. 
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A value of 40 centimeters per second was arbitrarily chosen for the vehicle velocity. For 
the simulation the initial vehicle body coordinates as x = 0, y = O, and the vehicle 
orientation as shown above 3*PI/4.0. Also, the coordinates of the wheels must be known. 
Hence, we place the vehicle wheels on sides that are 80 centimeters in length (like the 


Shepherd vehicle). 
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The last item is the structure required supporting the simulation [Ref. M], below 


is acode segment to illustrate the aforementioned statements. 


double deltaTime = 0.01; 
double Vel = 40.0; 
double omega = -0.1570796327; 


typedef struct { 
double x; 
double y; } 

POINT; 


typedef struct { 
POINT Point; 
double Theta; 
double Kappa; 
double Psi; 


j 
CONFIGURATION; 


q_init.Point.x = 0.0; 
qg_init.Point.y = 0.0; 
q init.Theta = 0.0; 
q_init.Kappa = 0.0; 
g_init.Psi = 2.356219449; /* 3*PI/4.0 */ 
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/ /individual wheel coordinates 
qfrontR.Point.x = 40; /* wheell */ 
qfrontR.Point.y = -40; 


qfrontL.Point.x = 40; /* wheel2 */ 
qfrontL.Point.y = 40; 


qrearR.Point.x = -40; /* wheel3 * / 
qrearR.Point.y = -40; 


qrearL.Point.x = -40; /* wheel 4 */ 
grearL.Point.y = 40; 


q_xaxis.Point.x = 0.0; /* line to be tracked, initial configuration */ 
q_xaxis.Point.y = 40.0; 

q_xaxis.Theta = 0.0; 

q_xaxis.Kappa = 0.0; 


Another key element in the structure CONFIGURATION is theta. Theta is 
simply the angle to the path being tracked. For instance if the vehicle is tracking a line, 
when the vehicle move onto the actual line then Theta’s value goes to zero. The final 
element required for the simulation is the step size that is used for the motion. The step 
size in the simulation code is called deltaS, and is vel*deltaTime (or .4 centimeters per 
second). Armed with this knowledge a simplified discussion can take place (for detailed 
knowledge of the compose function, and line tracking see Professor Kanayama’s motion 


planning and kinematics notes [Ref. 7]). 
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The compose function is used to determine (using deltaS ) the next position of the 
vehicle body. Here two compose functions are being used, one that is for the wheels 


(Compose2) and another for the vehicle body (Compose). 


CONFIGURATION Compose(CONFIGURATION& ql1,CONFIGURATION& 
q2, CONFIGURATION®& g3, double& s, double& deltaTime) 
{ double x,y, 

sinTheta = sin(q1.Theta), 

cosTheta = cos(q1.Theta); 


x = q].Point.x + q2.Point.x*cosTheta - q2.Point.y*sin Theta; 
y = ql.Point.y + q2.Point.x*sinTheta + q2.Point.y*cos Theta; 
q3.Point.x = x; 

q3.Point.y = y; 

q3.Theta = q1.Theta + q2.Theta; 


q3.Psi = ql.Psi + (omega * deltaTime); /* how to handle move left/right? */ 
fprintf(f6,"%10.3f %10.3f %10.3f %10.3f %10.3f\n", 
S\do Pome, d5.roint.y.qs. 1 hetay qairsi): 


return q3; 


}// end Compose 


CONFIGURATION Compose2(CONFIGURATION®& q1,CONFIGURATION& 
q2, CONFIGURATION& q3) /*position */ 
{ double x,y, 

sinTheta = sin(q1.Psi), 

cosTheta = cos(q1.Psi); 


x = gi.Point.x + q2.Point.x*cosTheta - q2.Point.y*sin Theta; 


y = q1.Point.y + q2.Point.x*sinTheta + q2.Point.y*cosTheta; 
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q3.Point.x = x; 
q3.Point.y = y; 


return q3; 
}// end Compose2 


The Compose function contains several lines that are important for observations 
in our simulation. The calculation of q3.Theta, as mentioned before the data theta should 
grow in positive manner as the vehicle moves from its initial point to the line above it. 
Once the line that is being tracked has been reached the value for theta goes to zero. 
Secondly the calculation for psi shows that as the vehicle moves from the initial position 
to the end of the line being tracked, the value of psi will be decremented by 
omega*deltatime (note in the code omega is defined as a negative number [Appendix 
M]). The Compose2 function is important because it provides the ability to compose the 
body orientation (psi) with the x and y cooridinates using the previously defined step. 

In the actual simulation the values of theta were manipulated to ensure the vehicle 
would track the next line above (40 centimeters higher) on the next step through the loop 


(see the code segment below): 


q_xaxis.Point-y = q_xaxis.Point.y + 40.0; 
if(1x%2==0) { 
q_xaxis. [Theta = PI; 


q.Theta = PI; 

omega = fabs(omega); 
else { 

q_xaxis.Theta = 0.0; 

q.Theta =0.0; 


omega = -omega; 


The simulation proved successful based on the data provided in Appendix L. Figure 6.4 
Is a graphical representation of the simulation data in Appendix L. Moreover, all the 
Structures are in place in the SRK [Appendix J, Consolidated header files] to implement 
the sensing motion. If more time was available for this thesis the sensing motion would 


have been implemented. 
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VII. CONCLUSIONS 


A. SUMMARY 


What was accomplished as a result of this research. A restatement of the goals or 
questions addressed of this is necessary. The thesis was to examine the following 
research areas: 


e What kinematics algorithms must be developed to support a vehicle with three 
degrees of freedom of motion? The aforementioned algorithms must support 
highly flexible, controlled, and precise motion. 


e What types of controls are required to ensure the optimal mix of driving and 
steering resources? Moreover, what must be done to ensure that all the 
resources complement? 


e How can the knowledge gained in the aforementioned research areas be used 
to develop searching motion? 


e How should the hardware and software systems be implemented to support 
the aforementioned goals? 


The kinematics algorithms developed to support a vehicle with 3 degrees of 
freedom of motion can be divided into 2 categories, ‘low level’ and ‘high level’. The first 
category 1S ‘low level’. The ‘low level’ algorithms are illustrated in Chapter IV and in 
Appendix E (motor.c). These ‘low level’ algorithms are concerned with taking inputs and 
passing the inputs to the hardware. The empirical results from the direct input and 
servomotor output can be seen in Chapter V. The ‘high level’ category considers desired 
body motion and its transformation into wheel motion (see Chapter IV and Appendix D). 
The driver function in movement.c binds the high and low level categories allowing for 
highly flexible, precise, and controlled motion of the rotary vehicle. 

The controls required for the optimal mix of driving and steering resources are 
developed in Chapter V. The algorithms and their implementation as described in 
Chapter V removes or lessens the effect of variance and disparities of servomotor output. 


This ensures an optimal mix of driving and steering resources and that resources are 


69 


utilized in a complimentary manner. Hence, the aforementioned algorithms gives the 
desired output within 1% error for performance specifications. 

The knowledge gained and the researched discribed in Chapters IV, V, and VI can 
be used to implement the required searching motion the rotary vehicle needs for the UXO 
effort. Discoveries and knowledge gained during development of the Tornado, 
Sinusoidal, and Joystick controlled motions; coupled with the searching motion 
simulation can be used to implement the tangential and searching motion (see 
Appendices C and D). Moreover, the structures are presently in place to support both 
tangential and searching motions in Appendix D. 

The hardware and software systems are implemented as described in Chapters III 
and IV. This hardware-software implementation is tightly coupled and ensures proper 
communication within the system. This communcation allows the users desired inputs to 


be translated into vehicle motion in real-time. 


B. LESSONS LEARNED 


The Shepherd rotary vehicle was designed by Professor Kanayama but built by 
Mitsubishi Heavy Industries in Japan. This resulted in product not to specification, and a 
lack of legible documentation. One aspect was that the rotary vehicle could not fit into 
the lab it was planned for. Because, the size of the robot was not correct. Also, the 
documentation provided was in Japanese, big problem. None of the authors could read or 
speak Japanese. This generated numerous faxes and telephone calls to Mitsubishi Heavy 
Industries requesting support and clarification. Numerous hours were lost in this 
endeavor. Moreover, some of the wiring diagrams did not match the actual wiring 
implementation on the rotary vehicle. 

The compiler used is GCC version 2.7.2.1. It was good because it was freeware. 
However, for cross-compiler requirements of the Motorola 68040 processor it left a lot to 
be desired. Most notably, the lack of all basic libraries. All input-output functions and 
math functions had to be written by the authors. Again, taking an inordinate amount of 


time and effort. Here the authors were recreating the wheel, per se. Also, the unusual 
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handling of structure passing required the development of several work-arounds. 
Essentially, standard C-code writing had to be modified extensively. The Motorola 
68040 chip was designed to support code written for the Motorola 68020. This may 
been true but the authors determined that for math functions this was not the case. This 
also may be related to the compiler (switches). The authors contacted Motorola and 
Omnibyte Corporations for help in dealing with these matters with little help provided. 
In the majority of cases the help provided led to dead-ends. Technicians at Omnibyte 
recommended that a purchase of a compiler suitable for the M68040 be made. 

The Taurus Bug (firmware on the Taurus Board) provided excellent capabilities. 
However, a lot of time was required to use the tools. On many occasions numerous hours 
were spent looking through loops of assembly code. This would not have been the case 
with a modern debugger. 

Group cross-pollenation is essential for a project with multiple disciplines 
involved. For example, a low voltage problem on the rotary vehicle caused the CPU to 
slow or halt on some occasions. Many hours were spent trying to determine what was 
wrong with the C-coding and the problem was hardware. This may have been alleviated 
had there been an electrical engineer on the team. Problems like this were exacerbated 
by the documentation problem mentioned earlier. Another problem was the overheating 
of the boards on the VMEbus (including the CPU) resulting in system shutdowns as well 


as aberrations in CPU behavior. 


C. RECOMMENDATIONS FOR FUTURE RESEARCH 


Recommendations for future research should include: 


e Implementation of tangential motion, the rudiments of which are already in 
place. 


e Implementation of sensing motion which is required for the percise, accurate, 
and safe detection of UXO's. 


e Transitioning the SRK to the PC environment possibly using LINUX freeware 
as the real-time operating kernal. 


A 


e Implementation of a M68040 specific compiler with all libraries. 


e Addition of the robot arm and implementation of the a neural net and expert 
system for UXO identification. Considerations should be given to sensors 
such as a magnetometer, digital camera devices, ground penetrating radar, 
xray devices and GPS. 


e Implementation of wireless ethernet or hand held computing devices for 
control and monitoring of the rotary vehicle by a user who is not co-located. 


e Possible missions other than UXO. For instance, an unmanned scout vehicle 
or mobile sentry. 


It is obvious from the above recommendations that the possible uses of the rotary vehicle 
is extensive and varied. The highly flexible, precise, and controlled motion of the vehicle 


makes it an ideal platform for many ground applications. 
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APPENDIX A: SOURCE CODE (MAKEFILE) 


The following code was modified by: Professor Kanayama, Thorsten Leonardy, 


Edward Mays, and Ferdinand A. Reid. 


| ff ----------- 2-2 -- nnn nn nnn nnn re nnn nnn nnn nnn nnn nnn nnn nnnn nnn nn nanan nn ce a 
2 # 5 
3 # File: MAKEFILE 3 
4 # * 
5 #Environment: GCC Compiler v2.7.2 ; 
6 # Update: QO2 February 1997 (Leonardy) . 
7 # O02 April 1997 (Ed Mays) ‘i 
8 # 14 May 1997 = (Leonardy, added utils030.*) 
9 # Name: Thorsten Leonardy 2 
10 # Purpose: Makefile for proyectSHEPHERD. si 
11 # 
12 # Invoke: make comp (to generate code) is 
13 # make print (to print all files to printer apl in Sp-511) * 
14 # make clean (to clean directory from object files) se 
1S # cs 
16 # --------------------------------------------------------------------------- | 
17 

18 


19 comp: startup.o shepherd.o timer.o serial.o math.o utils.o utilsO30.0 user.o \ 


20 motor.o movement.o 
21 Id -Ttext 0x 10000 -Tdata 0x20000 -Tbss 0x30000 -Map shepherd.map -oformat 
Ssrec \ 
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22 -o Shepherd.TXT startup.o shepherd.o timer.o serial.o math.o utils.o \ 
Ze utilsO30.0 user.o motor.o movement.o 
24 

25 shepherd.o : shepherd.c 

26 gcc -c -m68040 -o shepherd.o shepherd.c 
27 | 
28 timer.o : timer.c 

29 gcc -c -m68040 -o timer.o timer.c 

30 

31 serial.o : serial.c 

a2 gcc -c -m68040 -o serial.o serial.c 

33 

34 #servo.o : Servo.c 

35 # gcc -c -m68040 -o servo.o servo.c 

36 

37 utils.o : utils.c 

38 gcc -c -m68040 -o utils.o utils.c 

Bo 

40 utilsO30.0 : utilsO30.c 

4] gcc -c -m68040 -o utilsO30.0 utilsO30.c 
42 

43 user.o : uSser.c 

44 gcc -c -m68040 -o user.o user.c 

45 

46 motor.o : motor.c 


47 gcc -c -m68040 -o motor.o motor.c 
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48 


49 movement.o : movement.c 

50 gcc -c -m68040 -o movement.o movement.c 
= 

52 startup.o: startup.s 

53 as -o Startup.o startup.s 

54 

55 math.o: math.c 

56 gcc -c -m68040 -o math.o math.c 

Di) 

58 

59 # This cleans out everything except the Makefile, 
60 # and source files 

61 clean:; rm -f *.o core 

62 

63 # This prints all source files to the printer apl in Sp-511 


64 print:; enscript -2r -g -Pap] makefile shepherd.map shepherd.h shepherd.c \ 


65 user.h user.c utils.h utils.c utilsO30.c serial.h serial.c servo.h servo.c \ 
66 timer.h timer.c movement.h movement.c wheeldrive.h wheeldrive.c \ 
67 math.h math.c startup.s 

68 


69 # This prints all source files to the printer sp] in Sp-527 
70 prsp1:; enscript -2r -g -Psp] makefile shepherd.map shepherd.h shepherd.c \ 
A user.h user.c utils.h utils.c utilsO30.c serial.h serial.c servo.h servo.c \ 


ie timer.h timer.c movement.h movement.c navigat.h navigat.c wheeldrive.h 
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wheeldrive.c \ 
73 math.h math.c motor.h motor.c startup.s 


74 


76 # End of makefile 
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APPENDIX B: SOURCE CODE (SHEPHERD.C) 


The following code was modified by: Professor Kanayama, Thorsten Leonardy, 


Edward Mays, and Ferdinand A. Reid. 


Se rae Eero 8 eee eee 
pa * 
oe File: SHEPHERD .C - 
eee x 
5 * Environment: GCC Compiler v2.7.2 : 
6 * Last update: 29 January 1997 si 
7 * Name: Thorsten Leonardy 
8 * Purpose: Provides the kernel for SHEPHERD. ii 
Q * * 


10 * Compiled: >gcc -c -m68040 -o shepherd.o shepherd.c = 


ay . 
12 * ------------------------------------------------------------------------- *« / 
13 


14 #include "shepherd.h" /* general defines a) 
15 #include “movement.h" 


16 


21 unsigned char JOYSTICK[26]= { 


ah 


22 27,91,49,56,59,54,48,72, (ESC iol S600 ai 


2 120,61 ,48,48,48,32, x= OU) a 
24 121,61,48,48,48,32, /* 'y=000 ' aah 
2S 122,61,48,48,48,0}; /* '7=000' ey. 
26 


27 unsigned int wheelEncoder[8]; 
28 unsigned char bcedString[25]= 


29 {24,27,91,48,50,59,52,48,72, 
30 48 448 448 48 548 ,48 48,48,48,48,48,48,48,48,48,48 
3] 


32 /* clock contains the current date/time updated every second in ascii format */ 


33 /* it starts with a count byte, the cursor positioning sequence followed by */ 


34 /* the date-group and the time group in the form: ay 

BO ClOCch= > nce mm-dd-yy hh:mm:ss." where '.'is part of countor ESC */ 
260 “Sequence: o) 

a7 


38 /*unsigned char clock{27]={ */ 


39 /* 25,27,91,48,49,59,54,52,72, */ /* ESC['0''l'; '6''4'H */ 
40/* — 109,109,45,100,100,45,121,121,32, */ /* 'mm-dd-yy ' */ 
41 /* 104, 104,58, 109, 109,58,115,115,0};*/ /* ‘hh:mm:ss’ */ 
42 


43 void advanceCount() 
| 

45 edCounter++; 

46 } 

47 
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48 main() 


49 | 
50 
51 
52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63 
64 } 
65 
66 
67 


initBoards(); = /* initialize boards a7 
initMovement(); /* initialize movement 4) 
siolnit(); /* initialize 68C681 DUART for serial I/O ) 
timerStart();  _/* initialize and start timer-5 fopr motion control */ 


/* setup68030();*/ /* setup OMNI Module 0 for serial I/O to VT 100 ay 


enable(); /* enable all interrupts, user mode * / 

user(); /* let user handle the main portion 7) 
disable(); /* disable interrupts, supervisor mode */ 

/* here goes downloading stuff for analysis ... (i.e. copy memory ah 


/* from Taurus Main memory to host computer (Laptop or SparcStation) = */ 


returm; 


/* end of main() */ 


G8 [262 RRR GORI A CIC GI 2 ok ok ok a aC 2k ak a a a 2k 


69 End of shepherd.c 


70 em ee ee EE EEE EA AEE EET AREER ee ee Oe / 


5p 
WZ 
15 


1 


: 


' 
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APPENDIX C: SOURCE CODE (USER.C) 


The following code was modified by: Professor Kanayama, Thorsten Leonardy, 


Edward Mays, and Ferdinand A. Reid. 


pe nmninen ren ee = eee # 
> x ** 
oe File: USER ¢ = 
4 * * 
5 * Environment: GCC Compiler v2.7.2 _ 
6 * Last update: 18 February 1997 i 
7 * Name: Thorsten Leonardy : 
8 *Purpose: Provides the userpart for SHEPHERD. 
Q + # 
10 * Compiled: >gcc -c -m68040 -o user.o user.c si 
le * ; 
|2 * ---------------------------------------2 22 == $22 nono nnn nn nnn nn nnn nn nnn ei 
13 


14 #include "shepherd.h" /* general defines =] 


21 extern unsigned char inPortA; /* defined in serial.c */ 


22 extern unsigned char vtlOOxy[9]; /* ESC-Sequence for Cursor Position */ 


8] 


23 extern unsigned char clrSCR[5]; /* ESC-Sequence for clear Screen */ 
24 extern unsigned char clrLine[6]; /* ESC-Sequence for clear line  */ 
25 extern unsigned char prtSCR[4]; /* ESC-Sequence for print screen */ 
26 extern unsigned char cursorOFF[5];/* ESC-Sequence for cursor off */ 
ZY 

28 extern void gotoX Y (int,int); 

29 extern int wheelEncoder[8]; /* defined in shepherd.c */ 

30 

31 

32 extern char bcdString[]; /* defined in shepherd.c */ 

33 

34 unsigned short velocity=0; 


ae) 


40 

41 #define MENU_LINES 24 

42 char *menu[MENU_LINES ]={ 

43 “SHEPHERD Main Menu (Last Update: 27 Feb 97)\n\r", 
44 "|---5----0----5----0----5----0----5----0----5----0----5----0----5----0----5----O\n\r", 


45 "\n\r" 
46 "Please choose: Diagnostics\n\r", 
Ag: . \n\r", 


48  /* character pressed, analize ... */ 
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49 "(1) Stop nr > 
50 "(2) Straight Motion (Autonomous)  \n\r", 
51 "(3) Straight Motion by Joystick \n\r’, 


52 "(4) XY-Motion by Joystick \n\r", 
53 "(5) Rotate \n\r", 
54 "(6) Sinusoidal Motion \n\r", 


55 "(7) Tornado (External Center of Rotation)\n\r", 
S56 "(8) Tornado (Internal Center of Rotation)\n\r", 


57 "(9) Tangential Motion \n\r", 
58 "(0) Exit Program \n\r", 
59 "(a) Tangential Motion(II) \n\r", 
60 '"(t) Test wheel Angle 4 \n\r", 
61 "\n\r", 

eZ \n\r’, 

ooe  \n\r’, 

64 "An\r", 

Sone \N\r’, 

66 " \n\r", 

OQ] nnn ne nnn nnn nnn onan nnn nnn nnn nnn nnn nnn nnn nnn nn nn nnn nn nnn nnn nn enn nnn 
Gor” 

6? }; 

70 

7] 

2 

73 

74 
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76 * displayMenu() 
a) * k 
78 * Environment: GCC Compiler v2.7.2 - 
79 * Last update: 27 January 1997 ‘4 
80 * Name: Thorsten Leonardy i 
81 * Purpose: This function outputs a menu to the screen 3 
82 * * 
83 * ------------------------------------------ === $22 2-2-2 noe nnnn nen EL 


84 void displayMenu(void) 

Bot 

86 intl; 

87 sioOut(O,clrSCR); /* clear screen */ 
88 gotoX Y(1,1); 

89 for (G=0;1<MENU_LINES; i++) 

90 sioOut(O,menu[1]); 


2) tc tur: 

92 } 

5 

94 

5 

96 /* ------------------------------------------------------------------------- ie 
97 * status() ss 
oo : 
99 * Environment: GCC Compiler v2.7.2 a 
100 * Last update: 18 February 1997 i 


84 


101 * Name: Thorsten Leonardy 3 
102 * Purpose: _This function outputs a status line at the bottom of the * 


nos: * screen. : 


105void status(unsigned char *p) 

106 { 

107 gotoX Y(24,1); /* position cursor */ 
108 sioOut(0,clrLine); /* clearline */ 
109 sioOut(0,p); /* printtext */ 


110 return; 


111} 

112 

113 

114/* ------------------------------------------------------------------------- “i 
115 * convertBCD() : 
eno * 
117 * Environment: GCC Compiler v2.7.2 * 
118 * Last update: 20 February 1997 = 
119 * Name: Thorsten Leonardy = 


120 * Purpose: _‘ This function converts an unsigned integer to a BCD string * 


ea * of 16 characters. The value is right justified with leading * 
2 * Zeros. n 
So ee * / 


124void converttBCD(unsigned char *s, unsigned int data) 
125 { 
ZG Pint i=15; 
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W227 

128 for (i=15; i>=0; i--) { /* write 16 Bytes, adjust integer to right of string */ 
129 = *(s+1)=48+(data% 10); 

130 =data=data/10; 


iS 

132 return; 

NS} 

134 

135/* ------------------------------------------ 2-22 -------- $222 nanan : 
136 * convertInt() = 
ee 2 5 
138 * Environment: GCC Compiler v2.7.2 " 
139 * Last update: 11 June 1997 is 
140 * Name: Thorsten Leonardy, Yutaka Kanayama ‘J 


141 * Purpose: This function converts a signed integer toa BCD string * 


142 * of 16 characters. The value is right justified with leading * 
143 * Zeros. fe 
144 * ------------------------------------------------------------------------- i | 


14Svoid convertInt(unsigned char *s, int data) 
146{ 

147 int i=15; 

148 

149 if (data >= 0) 

IS0 = =*s=32; /* space _ for positive number */ 
Pyieselse 

Po. on 
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153 *s=45; /* minus sign for negative number */ 

154 = data = -data; 

i> } 

156 

157 for (i=15; i>=1; i--) { /* write 16 Bytes, adjust integer to right of string */ 
158 *(s+i)=48+(data% 10); 

159 = data=data/10; 


167 * wheelDrive() 7 
Gan . 
169 * Environment: GCC Compiler v2.7.2 a 
170 * Last update: 27 February 1997 a 
ive Name: Thorsten Leonardy - 


172 * Purpose: This function drives the specified wheel with required * 


ie * velocity. i 
174 * x 
RM oes eee ae ees ees Sl OE Oe Se eee * / 


176void wheelDrive 1 (unsigned short wheel, unsigned short velo) 


177{ 


178 unsigned short *servoOut=(unsigned short *)Oxffff0482; /* analog out */ 
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179 unsigned int *servoControl=(unsigned int*)Oxffffff00; /* Data out */ 
180 

181 *servoOut=velo; /* set velocity first */ 

182 

183 if (wheel) 

184 *servoControl=0x00000004; 

185 else 

186 *servoControl=0x00000000; 

187 


188 return; 


196 * updateWheelStatus() # 

oes i 

198 * Environment: GCC Compiler v2.7.2 z 

199 * Last update: 27 February 1997 i 

200 * Name: Thorsten Leonardy 2 

201 * Purpose: This function reads the current shaft encoder readings for * 
202 > all eight servo motors and outputs them to the screen. * 

203 = : 


204 * unsigned int wheelEncoder[8] - array to hold the shaft encoder readings * 
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205 * unsigned char *bcdString __ - string to hold converted encoder reading * 


206 * : 
207 * ------------------------------------------ 2-2-2 n nnn nn nn nn nnn nnn nnn a7 

208 void update WheelStatus(void) 

209 { 

210 

211 unsigned short 1,posx,posy; 

ZA 

213 readWheelStatus(wheelEncoder); /* read wheel status: File servo.c */ 
214 

215 posx=8; /* x-position on screen for reading motor | */ 
216 posy=40; /* y-position on screen for reading motor | */ 
ae 

218 for (i=0; 1<8; 1++) { 

219 =posx=8+1%4; /* position for x oy 

220 = posy=40+20*(1/4); /* position for y =i 

7) bedString[3 ]=48+posx/10; /* convert tens to ascil */ 
222 ~bcdString[4]J=48+posx% 10; /* convert ones to ascii */ 
223 ~— bcdString[6]=48+posy/10; /* convert tens to ascii */ 
224 i bcdString[7]=48+posy% 10; /* convert ones to ascii */ 
225. ~convertBCD(bcdString+9,wheelEncoder{i]); | /* convert reading itself */ 
226 WRITE_ENCODER(); /* output ascii =f 
221 } /* end of for */ 

228 

229) retum; 

230} 
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2 

32 

233void displayDriveS peed() 
234 { 

235 double speed00,speed0,speed! ,speed2,speed3; 
236 

237 disableQ); 

238 speedOO=desiredS peeds[0]; 
239 speedO=Display_Speeds[0]; 
240 speed!=Display_Speeds[1]; 
241 speed2=Display_Speeds[2]; 
242 speed3=Display_Speeds[3]; 
243 Display_Speeds[0]=0.0; 
244 Display_Speeds[{1]=0.0; 
245 Display_Speeds{2]=0.0; 
246 Display_Speeds{3]=0.0; 
247 

248 enable(); 

249 

250 —convertInt(bcdString+9,speed00); 
25 | bedString[3]='0'; 

252 bedString[4]='3'; 

255 bedString[6]='4'; 

254 bcdString[7]='0;; 

205 sioOut(0,bcdString); 


256 —convertInt(bcdString+9,speed0); 
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| 


bcdString[3]='0'; 


258 bcdString[4]='3’; 

259 _— bcdString[6]='6'; 

260 —_ bcdString[7]='0'; 

261 sioOut(0,bcdS tring); 

262 convertInt(bcdString+9,speed1); 
263 bcdString[3]='0'; 

264 bcdString[4]='4'; 

265 bcdString[6]='6'; 

266 bcdString[7]='0'; 

DOW sioOut(0,bcdString); 

268 convertInt(bcdString+9,speed2); 
269 i bcdString[3]='0'; 

270 __— bcdString[4]='5'; 

27 | bcdString[6]='6'; 

22 bedString[7]='0'; 

273 sioOut(0,bcdString); 

274 convertInt(bcdString+9,speed3); 
275 bcdString[3]='0'; 

ZG bcdString[4]='6'; 

90, bcdString[6]='6'; 

278 bcedString[7]='0'; 

279 sioOut(O,bcdString); 

280 return; 

281} 

282 


9] 


283 void displayDriveSteer() 

284 { 

285 double steer00,steer0,steer1 ,steer2,steer3; 
286 

287 disable(); 

288 steer00=Steer_Digits[0]; 

289 /* steerOO0=desiredAngleRates[0]; */ 
290 steerO=desiredAngleRates[0]* 1000; 
291 steerO=Display_Steers[0]; 

292 steerl=Display_Steers[1]; 

293 steer2=Display_Steers[2]; 

294 steer3=Display_Steers[3}; 

295 Display_Steers[0]=0.0; 

296 Display_Steers[1]=0.0; 

297 Display_Steers[2]=0.0; 

298 Display_Steers[3]=0.0; 

299 

300 enable(); 

301 

302 ~=convertInt(bcdString+9,steer00); 
303 ~— bcd SString[3]='0'; 

304. ~— bcd String[4]='3'; 

305. ~—s bc dString[6]='4'; 

306 bedString[7]='0'; 

307 sioOut(0,bcdString); 


308 ~~ convertInt(bcdString+9,steer0); 


2 


309 bedString[3]='0'; 

310 ~~ bedString[4]='3’; 

311 bedString[6]='6'; 

312 ~~ bedString[7]='0'; 

S15 sioOut(0,bcdString); 

314 convertInt(bcdString+9,steer| ); 
315 bedString[3]='0'; 

316 bedString[4]='4'; 

S47 bcdString[6]='6'; 

318 bedString[7]='0'; 

319 sioOut(0,bcdString); 

320 ~=convertInt(bcdString+9,steer2); 
Bc bcdString[3]='0'; 

322 ~— bcdString[4]='5'; 

a23 bcdString[6]='6'; 

324 ~~ bcedString[7]='0'; 

Bo sioOut(0,bcdString); 

326 convertInt(bcdString+9,steer3); 
a2 bedString[3]='0'; 

328 bedString[4]='6'; 

329 bcdString[6]='6'; 

330 bedString[7]='0'; 

33] sioOut(0,bcdString); 

332 return: 

333 } 

334 


os 


335void displayAngles() 

336 { 

337 double steer0,steer!,steer2,steer3; 

338 

339 disableQ; 

340 

341 steerO=actualAngles[{[0]* 1000*RadsToDegrees; 
342 steer!=actualAngles[1]*]000*RadsToDegrees; 
343 steer2=actualAngles[{2]*1000*RadsToDegrees; 
344 steer3=actualAngles[{3]* 1000*RadsToDegrees; 
345 

346 enable(); 

347 

348  convertInt(bcdString+9,steer0); 

349 bcdString[3]='0'; 

350 bedString[4]='3'; 

351 bedString[6]='6'; 

Bo2 bedString[7]='0'; 

353 sioOut(O,bcdString); 

354 ~=convertInt(bcdString+9, steer] ); 

B55 bedString[3]='0'; 

356 bedString[4]='4'; 

DS) bedString[6]='6'; 

358 bedString{7]='0’; 

359 sioOut(0,bcdString); 


360 convertInt(bcdString+9,steer2); 
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36] bedString[3]='0'; 

362 bcedString[4]='5'; 

363 bcdString[6]='6'; 

364 bcdString[7]='0'; 

365 sioOut(0,bcdString); 
366 ~=convertInt(bcdString+9,steer3); 
367 bedString[3]='0'; 

368 bcdString[4]='6'; 

369 bcdString[6]='6; 

370 bedString[7]='0'; 

371 sioOQut(0,bcdString); 
S72 return; 

B73) 

374void display VehicleConfig() 
375 { 

376 double coordx, coordy, heading, kappa; 
377 

378 disable(); 

579 

380 coordx = vehicle.coord.x; 
381 coordy = vehicle.coord.y; 
382 heading = vehicle.heading; 
383 kappa = vehicle.kappa; 
384 

385 enable(); 

386 
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387 


convertint(bcdString+9,coordx); 


388 bcdString[3]='0'; 

389 bcdString[4]='3'; 

390 bcdString[6]='6'; 

39] bedString[7]='0'; 

392 sioOut(0,bcdString); 

393 ~convertInt(bcdString+9,coordy); 
394 __— bcd String[3]='0'; 

395 bedString[4]='4'; 

396 _— bcdString[6]='6’; 

397 bcdString[7]='0'; 

398 sioOut(0,bcdString); 

399 convertInt(bcdString+9,heading); 
400 __ bedString[3]='0'; 

401 bedString[4]='5’; 

402 _ bcdString[6]='6'; 

403 bedString[7]='0'; 

404 sioOut(0,bcdString); 

405 convertInt(bcdString+9,kappa); 
406 _ bcdString[3]='0'; 

407 bedString[4]='6'; 

408 bcdString[6]='6'; 

409 bedString[7 |='0'; 

410 sioOut(0,bcdString); 

411 retum; 

412} 
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414 * user() xs 

AlS * zt 

416 * Environment: GCC Compiler v2.7.2 
417 * Last update: 18 February 1997 & 
418 * Name: Thorsten Leonardy - 

419 * Purpose: This function provides the user shell. ss 


420 * - 


422 

423 void user(void) 

424 { 

425 inta:; 

426 char *s; 

427 unsigned int *servoControl=(unsigned int *)VME2170;/* test only */ 


428 displayMenu(); /* display menu ey 


429 do 
430 { 
43] inPortA="""; /* reset character a) 


432 ~~ while(inPortA=='?’); /* wait for character to be typed in */ 
433 /* character pressed, analize ... */ 

434 — switch(inPortA) 

435 { 

436 Case eeeimode |= 5)) By Stopes 

437 { 


438 modeOstate = O; 
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439 mode = 1; 


440 while (modeOstate ==0) { }; 
44] disable(); 

442 align(); 

443 enable(); 

a4 

445 

446 

447 

448 

449 /* *servoControl=0x00429429; test by Ed */ 
450 modeOstate = 2; 

45] } 

452 else 

453 { 

454 mode = 1; 

455 disable(); 

456 alignAfterRotate(); 

457 enable(); 

458 /* *servoControl=0x00429429; test by Ed*/ 
459 

460 } 

461 initMovement(); 

462 break; 

463 


464 case ‘2’: mode = 2; /* Straight Motion (Autonomous) */ 
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465 break; 

466 

467 case '3': mode = 3; /* Straight Motion by Joystick */ 
468 break; 

469 

470 case '4': mode = 4; /* X-Y Motion by Joystick */ 


471 /* for (a=0;a<100;a++){ af 

472 /* while ((edCounter % 200 != 0) && (a != 100)){ */ 
473 he displayAngles(); i 

474 a “i 

475 je a 

476 break; 

477 


478 case '5': modeSstate =Q0; /* Rotate | 


479 mode = 5; 
480 break; 
481 


482 case '6' : mode = 6; /* Sinusoidal Motion */ 
483 break; 
484 


485 case ‘/': mode = 7; /* Tornado (Center of Rot External) */ 


486 break; 

487 

488 case '8' : mode = 8; /* Tornado (Center of Rot Internal) */ 
489 break; 

490 
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491 
492 
493 
494 
495 
496 
497 
498 
499 
500 
501 
502 
503 
504 
505 
506 
507 
508 
509 
510 
511 
512 
513 
514 
515 
516 


case '9': mode = 9; /* Tangential Motion */ 
initTangent(); 
while(1){ 
while(edCounter%200 != 0){ }; 
display VehicleConfig(); 
}; 
break; 


case ‘a’ : mode = 10; /* Tangential Motion (II) */ 


break; 


case 't': modeTstate = 0; /* Steering test mode */ 
/* Flag = 1; initialized in movement.c */ 
mode = 100; 
while (1) 
{ 
oldFlag = Flag: 
while (Flag == oldFlag) { } 
displayAngles(); 
} 
break; 


default : break; 


} /* end of switch */ 


} while(inPortA!='0');/* end of while, exit with 'O' entered at keyboard */ 
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517 sioOut(O,clrSCR); = /* clear screen */ 
518 sioOut(O,"\n\r\n\r"); /* some cr,lf */ 
so 

a) retum; 

a2) 

522 while(1) 

eo = C{ 

524 while(edCounter%200 != 0){ }; 

525 /* displayJoyStickQ); */ 

526 displayDriveSteer(); 

Be /* displayAngles();  */ 

528 —}; 

929 sioOut(O,cursorOFF); /* switch cursor off (no blink) */ 
530 

531} /* end of user() */ 


Doe 

533 

534 
535asm(" 
536 _.even 
Dey) .text 


538 = .glob] LWRITE_LENCODER 
Bo 

540_WRITE_ENCODER: 

54] 


542 pea.! _bcdString 
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543 

544 = trap #15 
545. — dc.w 0x0023 
546 

547 rts 

548"); 

549 

550 

55) 


Sot ttt ete eet EERE EEE EERE EERE RE REREEE EE EE Ee ERE eet hoe 


553 End of user.c 


S54 TEESE ELEL EEE EE EEE EERE RARER REET E ERE ERE EEE EEE EEK EE EAE EEE / 


323) 
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APPENDIX D: SOURCE CODE (MOVEMENT.C) 
The following code was modified by: Professor Kanayama, Thorsten Leonardy, 


Edward Mays, and Ferdinand A. Reid. 
1 #include "shepherd.h" 
2 #include "movement.h" 


3 #include "math.h" 


4 
Re i i eee eee eee 2k 
6 * Main *k 

Re 8 renee Schoen a ccccucaceeasesdeuee eee x / 


8 void driver() 

oN 

10 readEncoders(); /* Read Drive/Steer Motors */ 

11 computeActualRates(); 

12 /*accumulateDriveSpeed(); only for wheel speed displaying */ 
13. accumulateDriveSteer(); 

14. bodyMotion(); 

15 ~wheelMotion(); 

16 /* testDrivel(); */ 

17 driveMotors(); 


18 advanceCount(); 
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21 

DD Rees cee ee SSeS ee ee ee eee * 
23 * Initialize Movement: ss 
24 * intialize Configuration and vehicle motion . 
Doi 8 oie se Sere o ena re an cates cee ee wee tee eases sea * / 


26 void initMovement() 
7) 

28 int 1x; 

29 

Boe Flag = 1; 

31  oldMode = 0; 

BZ ends =a. 

33. Omega_Speed=0.0; 
34  testCounter=0; 

35 edCounter=0; 

36 pathLength=0.0; 

37 

38 KI(OJ=11.448;/*0<=speed<=5,*/ 
52 SR ies: 

40 K1[2]=11.496, 


41 K1[3)J=12.375; 


104 


42 


43 


44 


45 


46 


47 


48 


49 


a0) 


>) 


52 


5 


54 


=) 


56 


a7 


58 


ay 


60 


61] 


62 


63 


K2[0]=11.500;/*5>speed<8*/ 
K2(1J=11.500; 
K2[2]=1 1.644; 


K2[3]=12.000; 


K3[O]=11.611;/*8>=speed<20*/ 
reel 1585: 
K3[2]=1 1.686; 


K3(3]=1 1.840; 


K4(0]=11.711;/*20>=speed<=70*/ 
K4[1]=11.659; 
Fe4)2)=11.705; 


K4[3]=1 1.727; 


K6[OJ=11.710;/*70>speed<K5*/ 
K6[1]=11.700; 
K6[(2]=1 1.700; 


K6[3)J=11.715; 
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64 


65 


66 


67 


68 


69 


70 


71 


o2 


73 


74 


iS 


76 


77 


78 


uo 


80 


8 | 


83 


84 


DigitloCmDrive[0]= +0.001 1369287;/* driving constant cm/count = 
digitlfoRadDrive* 18.9cm*/ 

DigitToCmDrivel 1]= -0.001 1369287; 

DigitfoCmDrive[2]= +0.001 1369287; 


DigitToCmDrive[3 ]= -0.001 1369287; 


motion.Speed=0.0; 
motion. Theta=0.0; 


motion.Omega=0.0; 
radius = 100; 

vehicle.coord.x=0.0; 
vehicle.coord. y=0.0; 


vehicle.heading=0.0; 


vehicle.kappa=1/radius; 


ai[O} = 40.0; ai[ 1] = 40.0; ai[2}] =-40.0; ai[3] =-40.0: 


bi{O] =-40.0; bi[1] = 40.0; bi[2] =-40.0; bi[3] = 40.0: 
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85 


86 


87 


88 


89 


90 
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92 


OS 


94 


YS 


96 


ay) 


98 


2) 


100 


101 


102 


103 


105 


106 


joyStick.x = 0.0; 


joyStick.y = 0.0; 


setupPolar(whp); 


for (ix =0; ix <ARRAY_SIZE; ix++){ 
PreviousCountSpeed[ix ]=99999999; 
PreviousCountSteer[ix ]=99999999: 
Display_Speeds[ix]=0.0; 
Display_Steers[ix]=0.0; 
actualAngles[1ix]=0.0; 
desiredSpeeds[ix] = 0.0; 
desiredAngleRates[ix] = 0.0; 
desiredAngleRatesO[ix] = 0.0; 
desiredAngles[ix ]=0.0; 
desiredAnglesO[ix ]=0.0; 
WheelDirActOfix]= 1.0e8; 
WheelDirAct[ix] = 0.0; 
WheelDirDes[ix] = 0.0; 
steerReadings[1x ]=0.0; /* not used only testing */ 


driveReadings[ix]=0; 
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Oe 
108 } 
109 


110 


114void setupPolar(polar whp[4]) 
fale 


116 whp[O].rho = whp[1].rho = whp[2].rho = whp[3].rho = 56.5685425; 


117 /* distances = 40 * sqrt(2) */ 
118 whp[0).alpha = -QPI; /* front right wheel | */ 
119 whp[l1].alpha= QPI; /* front left wheel 2 */ 


120 whp[2].alpha = -3.0*QPI; 7 Teak nent wheel 2) 
12). whp[3].alpha= 3.0*QPI; /* rear left wheel4 */ 
2 
123 


124 


128 void bodyMotion() 


108 


129{ 

130 double vO, omega0, 

131 linSpeed= 4.0, 

32 linAcc = 1.0, 

133 rotSpeed= 0.1, EOS, a 

134 rotAcc =0.025, 1-0-0125; 

5 RPI =QPI*1.5;  /* 67.5 degrees */ 

136 double theta, omega, speed; 

137 

138 speed = motionO.Speed = motion.Speed; /* save the previous motion */ 
139 theta = motionO.Theta = motion.Theta; /* for computing derivatives */ 
140 omega = motionO.Omega = motion.Omega; 

141 

142 switch(mode){ 

143 ~—s case |: 

144 if (modeOstate == 2) 

145 break; 


146 if ( (Speed_Digits[0] == (0) && (Speed_Digits[1] == 0) && 


147 (Speed_Digits[2] == 0) && (Speed_Digits[3} == 0) && 
148 (Steer_Digits[O] == 0) && (Steer_Digits[1] == 0) && 
149 (Steer_Digits(2] == 0) && (Steer_Digits[3] == 0)) 

150 modeOstate = 1; 


109 


Su 


es 


IS 


154 


2 


156 
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158 


159 


160 


161 


162 


163 


164 


165 


166 


167 


168 


169 


170 


171 


172 


/* allStopQ); will be inserted later */ 


break; 


case 2: 


speed = min(speed + 2.0*DeltaT, 10.0); 


break; 
case 3: /* Straight Motion by Joystick */ 
readJoyStick(); /* eym 19 july 97 */ 


speed = -joyStick.y*0. 1; 
theta = 0.0; 
omega = 0.0; 


break; 


case 4: /* X-Y Motion by Joystick */ 


readJoyStick(); p seine Oly O77 

speed = -joyStick.y*0.1; | /* speed control */ 

theta = -joyStick.x*0.02; /* steering control */ 

if (theta > HPI) theta= HPE 

if (theta <-HPI) theta = -HPI; 

/* omega = -joyStick.omega*0.1;*/ /* 24 july 97 */ 


break; 
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73 


174 


175 


176 


177 


178 


179 


180 


181 


182 


183 


184 


iso 


186 


187 


188 


189 


190 


2 


ie 


193 


194 


CASe oO: 
if (modeSstate == 1){ 
readJoyStick(); 


speed = -joyStick.y*0. 1; 


break; 


case 6: /* sinusoidal motion */ 
speed = min(speed + linAcc*DeltaT, 10.0); 
speed = speed; 
i speed —— 10:0) 
pathLength += DeltaT*speed; 
theta = 0.4 * sin(pathLength/20.0); /* sine curve motion */ 


break; 


case 7: /* Tornado External */ 
speed = min(speed + 1.0*DeltaT, 8.0); 
ie speed ——6, 0) 
omega = min(omega + 0.0125*DeltaT, 0.1); /* radius = 80 cm */ 


break; 


11] 
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205 


206 
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208 
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213 


case 8: /* Tornado Internal */ 


speed = min(speed + 1.0*DeltaT, 8.0); 


if ( speed == 8.0) 


omega = min(omega + 0.025*DeltaT, 0.2); 


break; 


case 9: /* tangential motion */ 


tangentialMotion(); 


break; 


case 10: /* tangential motion (II) */ 


speed = min(speed + linAcc*DeltaT, 8.0); 


break; 


case 100: 


break; 


if (mode != 9){ 
motion.Speed = speed; 


motion.Theta = theta; 


Ie 


/* radius = 40 cm */ 


217  motion.Omega = omega; 

218 

219 ~~ vehicle.heading = vehicle.heading + motion.Omega*DeltaT; 

220 ~_—vehicle.coord.x = vehicle.coord.x + motion.Speed*DeltaT * cos(motion.Theta); 
221 ~~vehicle.coord.y = vehicle.coord.y + motion.Speed*DeltaT * sin(motion.Theta); 
Dee 

223 speedDot=(motion.Speed - motionO.Speed)/DeltaT; 

224 ~ thetaDot=(motion.Theta - motionO.Theta)/DeltaT; 

225. omegaDot=(motion.Omega - motion0.Omega)/DeltaT; 

226° } 

227} 

228 


e29 


233 void wheelMotion() 

234{  /*the function that truly belongs here is in calculate.org */ 
Zoo) int 1; 

236 double vix, vly, vl yv1xRatio; 

237 double theta=motion.Theta, 


238 omega=motion.Omega, 


iS 


239 speed=motion.Speed, 

240 Omega2=omega*omega, 

241 Omega3=Omega2*omega, 

BEDS beta,ro,ro2, 

243 wheelAngleV; 

244 

245 if (mode == 5){ /* rotate case */ 


246 = switch(modeSstate) { 


247 case 0: 

248 /* turn each wheel by +-PI/4 in 5 seconds */ 
249 desiredAngles[0O] += QPIby500; /* = (PI/4)/500 */ 
250 desiredAngles[1] -= QPIby500; 

Zoi desiredAngles[2] -= QPIby500; 

Zo. desiredAngles[3] += QPIby500; 

23 if (desiredAngles[0O] >= QPI) 

254 modeSstate = 1; 

25> break; 

256 

25) case |: /* drive wheels to rotate body */ 
258 desiredSpeeds[0] = +speed; 

Zo desiredSpeeds[1] = -speed; 

260 desiredSpeeds[2] = +speed; 
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26] 


262 


263 


264 


265 


266 


267 


268 


269 


270 


Zi 


22 


273 


274 


Ze 


216 


280 


28 | 


282 


desiredS peeds[3] = -speed; 
break; 


retum; 


for (=0; 1<4;1++){ /* non-rotate case */ 
ro=whp{i].rho; 
ro2=ro*ro; 
beta=vehicle.heading+whp[i].alpha; 
vlx = speed*cos(theta)-(whp[i].rho*omega*sin(beta)); 
vly = speed*sin(theta)+(whp[1i].rho*omega*cos(beta)); 


desiredSpeeds{i] = new_sqrt(v1x*v1x + vly*vly); 


switch(mode){ 
case |: 
case 2: 
Ease 3. 
if (speed < 0.0) 
desiredSpeeds[i] = -desiredSpeeds[1]; 
if (new_abs(v1x) > 0.01){ 


Vlyv1xRatio=vl y/v1x; 


iS 


ooo 


284 


285 


286 


Zo 


288 


So 


290 


291 


geZ 


25 


294 


yo) 


296 


29) 


298 


299 


300 


301 


303 


304 


desiredAngles[i] = atan(vl yv1xRatio) - vehicle.heading; 
wheelAngleV = motion.Theta - vehicle.heading - whp[1].alpha; 
desiredAngleRates[i] = 
( (speed*speed*thetaDot + ro2*Omega3) 
+speed*ro*omega*(omega+thetaDot)*sin(wheelAngleV) 
+ro*(omegaDot*speed-omega*speedDot)*cos(wheelAngleV) ) 
/( desiredS peeds[i]* desiredSpeeds[i]) - omega; 
desiredAnglesO[i] = desiredAngles[i]; 
desiredAngleRatesO[i] = desiredAngleRates[1]; 
else { 
desiredAngles[i] = desiredAnglesO[1]; 
desiredAngleRates[i] = desiredAngleRatesO[1]; 


} 


break; 


case 4: 
if (speed < 0.0) 
desiredSpeeds[1i] = -desiredSpeeds[i]; 
if (new_abs(v1x) > 0.01){ 
VlyvIxRatio=vly/v1x; 


desiredAngles[i] = theta; 
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305 


306 


307 


308 


309 


310 


311 


S12 


313 


314 


315 


316 


S17) 


318 


319 


320 


B21 


B22 


desiredAngleRates[i] = 0.0; 

desiredAnglesO[i] = desiredAngles[1]; 

desiredAngleRatesO[1] = desiredAngleRates[1]; 
else { 

desiredAngles[1] = desiredAnglesO[1]; 


desiredAngleRates[i] = desiredAngleRatesO[1]; 


break; 


case 6: 
case 7: 
case 8: 
case 9: 
if (new_abs(desiredSpeeds[i]) > 0.01) { 
desiredAngles[i] = atan2(vly,v1x) - vehicle.heading; 
wheelAngleV = motion.Theta - vehicle.heading - whp[i].alpha; 
desiredAngleRates[i] = 
( (speed*speed*thetaDot + ro2*Omega3) 
+speed*ro*omega*(omega+thetaDot)*sin(wheelAngleV) 
+ro*(omegaDot*speed-omega*speedDot)*cos(wheelAngleV) ) 


/( desiredSpeeds[i]* desiredSpeeds[1]) - omega; 
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aed 


328 


a2? 


330 


ao 


332 


So 


334 


5955 


336 


337 


338 


57 


340 


34] 


342 


343 


344 


345 


346 


347 


desiredAnglesO[1] = desiredAngles[1]; 
desiredAngleRatesO[i] = desiredAngleRates[1]; 
} 
else{ 
desiredAngles{i] = desiredAnglesO[i]; 


desiredAngleRates[i] = desiredAngleRatesO[1]; 


break; 


case 10: 
desiredSpeeds[i] = speed * 
(new_sqrt((ai[i]*vehicle.kappa)*(ai[i]*vehicle.kappa) 
+(1-bi[{i]*vehicle.kappa)*(1-bi[i]*vehicle.kappa))); 
if (vehicle.kappa != 0.0) { 
desiredAngles[1] = atan2(bi[i],(vehicle.kappa-ai[i])); 
} 
else { desiredAngles[i] = 0.0; } 
desiredAngleRates[i] = 0.0; 
break; 
case 100: 
break; 


}/* end switch */ 
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348 }/* end for */ 
349 } 

350 

35) 


BDZ 


356void joystickMotionInterface() 
B57 { 
358 motion.Speed = joyStick.y; /* convert x-position into double */ 


359 motion.Theta = joyStick.x; /* convert y-poistion into double */ 


360 motion.Omega =0.0; /*motion.Omega = joyStick.w; not implemented yet;*/ 
361} 

362 

re 2 2a nom one cansenenwaunsns oe = ** 

364 * tangentialMotion - 

Se re naa nnn stab eee ee */ 


366void tangential Motion() 

Bor 

368 double deltaTheta, deltax, deltay, Si, totalDistance, deltaDistance; 
369 int 1x; 


ele 


370 


a7] 


i 
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374 


oD 


376 


oy 


378 


379 


380 


38 | 


382 


383 


384 


385 


386 


387 


388 


389 


390 


39] 


deltax = 0; 
deltay = 0; 
for (ix = 0; 1x < 4; 1x++){ 
deltax = deltax + actualSpeeds[ix]*cos(actual Angles[ix]); 


deltay = deltay + actualSpeeds[ix]*sin(actualAngles[ix]); 


/*returns the linear distance the vehicle has travelled */ 


deltaS = (DeltaT/4)*new_sart((deltax*deltax)+(deltay*deltay)); 


/* returns the difference between the changes in the distance */ 
/* of the left and right wheels a 
deltaTheta = 0.0; 
for (1x = 0; 1x < 4; 1x++){ 

Si = actualSpeeds[1x]* DeltaT; 

deltaTheta = deltaTheta + (sin(actual Angles[1x])/ai[ix] 

- cos(actual Angles[1x])/bi[ix])*S1; 

} 


deltaTheta = deltaTheta/4; 


totalDistance += deltaS; /* Keeps track of the total distance traved by vehicle */ 
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Bo2 

393 /* update the vehicle's configuration based on the distance travelled */ 
394 /* during the last motion control cycle | 
395 

396 vehicle.heading += deltaTheta; 

397 circularArc(deltaS, deltaTheta); 

398 compose(); 

399 

400 deltaDistance = DeltaT*motion.Speed; 

401/* vehicle.kappa += (steer()*deltaDistance);*/ 

402 vehicle.kappa = 0.0; 

403/* motion.Theta += deltaTheta; t/ 

404/* motion.Theta = vehicle.heading; */ 

405 motion.Theta = 0.0; 

406/* motion.Omega = vehicle.kappa*motion.Speed; */ 

407 motion.Omega = 0.0; 

408 thetaDot = deltaTheta/DeltaT; 

409 speedDot = 0.0; 


410 omegaDot = 0.0; 


12] 


414/* 


415/* 


416/* 


ali 


418/* 


419/* 


420/* 


421/* 


422/* 


423/* 


424/* 


a) * 


426/* 


427/* 


428/* 


429/* 


FUNCTION: circularArc() TH 
PARAMETERS: Configuration length --the arc length ay 
alpha --the end orientation oH 


config --pointer to the resultant configuration */ 


PURPOSE: Given the arc length and alpha, to calculate the final uf 
configuration oy 
RETURNS: Configuration: pointer to the final configuration ay 


COMMENTS: The main purpose of this function is to be used in conjunction */ 
with compose() to form a new next(). In this case, length would*/ 
actually be delta-s and alpha would be delta-theta. i 


Circular_arc() would determine the configuration after the incre-*/ 


mental move in the local coordinate system of the original ss, 
configuration. Then compose() would take the original 5) 
configuration (in global coordinates) and the incremental */ 


configuration (in local coordinates) to determine the */ 


incremental configuration in global coordinates. a 


430/70 ARCA CCC CSI GCI ICG ICI a AG AI A / 


431 void 


432circularArc(double length, double alpha) 


433 { 


434 


435 double alpha2,alpha4; 


436 
437 alpha2 = alpha*alpha; 
438 alpha4 = alpha2*alpha2; 


439 defineConfig((1- alpha2/6.0 + alpha4/120.0) * length, 


440 (0.5 - alpha2/24 + alpha4/720.0) * alpha * length, 
44] alpha, 0.0); 
442} 


444/* FUNCTION: defineConfig() a 


445/* PARAMETERS: double x, y,theta,kappa --The values that definea */ 


446/* configuration “| 
447/* PURPOSE: To allocate nad assign a configuration =) 
448/* RETURNS: Configuration: aconfiguration zh 
449/* COMMENTS: Was called def_configuration() in MML10 =) 


451 void 

452 defineConfig(double x,double y,double theta,double kappa) 
453 { 

454  incrementalMotion.coord.x = x; 

455 incrementalMotion.coord.y = y: 

456 incrementalMotion.heading = theta; 


457 incrementalMotion.kappa = kappa; 


23 


458} 
459 


460 
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462/* 


463/* 


464/* 


465/* 


466/* 


467/* 


468/* 


469/* 


470/* 


471/* 


472/* 


473/* 


474/* 


475/* 


476/* 


FUNCTION: compose() a, 
PARAMETERS: Configuration *first -- pointer to the first configuration 7) 
*second -- pointer to the second configuration */ 

PURPOSE: To calculate the composition of the first and second ey 
configurations ey 

RETURNS: Configuration: configuration which is the tf 
composition of the first and second configurations tf 

COMMENTS: A typical example of the usage of this function is to determine */ 


the goal position of a configuration in global coordinates. In */ 

such an example, the first argument would be the original ay 
configuration and the second argument would be the goal tf 
configuration in the original configuration's local coordinate = */ 
system. The resultant third argument would then be the goal = */ 
configuration in global coordinates.Was called comp() in MMLI10 


LAST UPDATE: 10/25/94 Chien-Liang Chuang 


a 


a 
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478void 


479compose() 


124 


480{ 

481 

482 double x,y, theta; 

483 double xx,yy,tt; 

484 

485 holdVehicle.coord.x = vehicle.coord.x; 

486 holdVehicle.coord.y = vehicle.coord.y; 

487 holdVehicle.heading = vehicle.heading; 

488 holdVehicle.kappa = vehicle.kappa; 

489 

490 x = incrementalMotion.coord.x; 

491 y =incrementalMotion.coord.y; 

492 theta = holdVehicle.heading; 

493 

494 

495 xx =cos(theta) * x - sin(theta) * y + holdVehicle.coord.x; 
496 yy =sin(theta) * x + cos(theta) * y + holdVehicle.coord.y; 
497 

498 tt = holdVehicle.heading + incrementalMotion.heading; 
499 

500 vehicle.coord.x = xx; 


501 vehicle.coord.y = yy; 


is 


502 vehicle.heading = tt; 

503 vehicle.kappa = holdVehicle.kappa; 

504 } 

505 

5.06 [GBA SSC SCO IO ICICI ICI IRI I II ICA I AICI 1 I 1 4 21 1 8 1 A A CO Ck / 
507/* FUNCTION : steer(robot,line) PURPOSE : evaluate steering = 
508/* function ah 
509 22GB RAG dc GG GaGa I I SGI RI ICI A ICICI I IK AC I I Ak 1 i 2k a 2k ak ak ak ak ak / 
510double steer() 

a) 

512{ 

513 double lambda, angle, dist; 

514 

515 if (currentPath.config.kappa == 0.0) 


516 lambda = - currentPath.a * vehicle.kappa 


Bally - currentPath.b * norm(vehicle.heading - currentPath.config.heading) 
518 - currentPath.c *(-(vehicle.coord.x - currentPath.config.coord.x) 

519 * sin(currentPath.config.heading) 

SVAy) +(vehicle.coord.y - currentPath.config.coord.y) 

al * cos(currentPath.config.heading)); 

522 else 

a23 0a 
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524 angle = Psi(vehicle.coord, currentPath.center); 
525 dist = distance(currentPath.center, vehicle.coord); 


526 if (currentPath.config.kappa > 0.0) 


S20 { 

528 lambda = - currentPath.a * (vehicle.kappa-currentPath.config.kappa) 
ao - currentPath.b * norm(vehicle.heading-(angle-HPI)) 

530 - currentPath.c * (currentPath.radius - dist); 

5311 } 

Bee else 

523 lambda = - currentPath.a * (vehicle.kappa-currentPath.config.kappa) 
534 - currentPath.b * norm(vehicle.heading-(angle+HPI) 

52D - currentPath.c * (currentPath.radius + dist); 

B36 CO} 


Sf return lambda; 

538 } 

oo 

5.40 2A AB AAG ISOC CIO IIORI I IOI II II IC II ICICI AI I 1 I I 1 A 1 2K 2 Ok 2K / 
541 void constants() 

542 { 

543 double k; 

544 


545 k = 1.0/sigma; 


ee 


546 currentPath.a = 3.0*k; 
547 currentPath.b = 3.0*k*k; 
548 currentPath.c = k*k*k; 


549 } 


551/* Function: Psi_function() a 
552/* Purpose: Computes the Psi function of two given points ou 
553/* Parameters: point p1,p2 a 
554/* Returns: double 7 
555/* Comments: oi 


557 double 

558Psi(point pl ,point p2) 

550 

560 { 

S61 if (p2.y - pl.y ==0.0 && p2.x - pl.x == 0.0) 
562 return 0.0; 

SCs cise 

564 return atan2(p2.y - pl.y, p2.x - pI.x); 

565 } 

566 


567 


568 /FEREEEERA EERE ERERKEEAAAA AAA RRA EE EE ER EAE AK RRR KKK AEE KKK KKK EK f 


569/* Function: distance() 


570/* Purpose: Computes the distance between two given points 


571/* Parameters: point pl,p2 
572/* Returns: double 


573/* Comments: 


a 


a 


oy 


i 


oy 


rane ast le nae che he ots ne hs ots 2 i hy os he os RoR Ae OR Oe OR Oe Ae OE 2 REE Oe Ae Oe A Be oho as oh ie oI os ols Ae ots ots oe iets ac Ae os oA OB oR ooh oh / 


575double 

576distance(point pl ,point p2) 

Si 

578 { 

579 double X, Y; 

580 

581 X= pl.x - p2.x; 

982 Y =pl.y- p2.y; 

583 return new_sqrt( X*X + Y*Y ); 
584} 

555 

S86void initTangent() 

587 { 

588 currentPath.config.coord.x = 0.0; 


589 currentPath.config.coord.y = 0.0; 
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m2) 


| 


59) 


593 


594 


595 


596 


oi 


598 


a0 


600 


601 


602 


603 


604 


605 


606} 


607 


currentPath.config.heading = 0.0; 
currentPath.config.kappa = 0.0; 
currentPath.radius = 0.0; 
currentPath.center.x = 0.0; 
currentPath.center.y = 0.0; 

sigma = 20.0; 


constants(); 


motion.Speed = 10.0; 


motion.Theta = 0.0; 


motion.Omega = 0.0; 


vehicle.coord.y = 0.0; 


vehicle.coord.x = 0.0; 
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APPENDIX E: SOURCE CODE (MOTOR.C) 


The following code was modified by: Professor Kanayama, Thorsten Leonardy, 


Edward Mays, and Ferdinand A. Reid. 


2 // Edward Mays 
3 // Shpeherd project 
4 // 20 February 1997 


5 // MotionControl 


8 #include 'motor.h" 

9 #include "shepherd.h" 

10 #include "math.h" 

1] 

12 void readEncoders() { 

13. readDriveEncoders(driveReadings); 

14 readSteerEncoders(steerReadings); 

f} 

16 

ie ef 

18 /* Verifies validity of incoming speeds/angles and converts 
19 /* digitial input for the DA board ve 
ZO) * ay 

21 void driveMotors(){ 

a 

23. int ix,Speed_Digit,Steer_Digit, counter; 


131 


mh 


24 
as 
26 
ea 
28 
Zo 
30 
3] 
Sy 
2 
34 
2 
36 
o7 
38 
59 
40 
4] 
42 
43 


45 
46 
47 
48 
49 


double speed1, steer1, temp; 


unsigned short bitMask=0x8000;  =/* access bit 15 for align wheel | */ 


unsigned short *servoStatus=(unsigned short *)(VME9421+0x00ca); /* digital input */ 


bitMask = bitMask >> 3: 


/* updateWheelDrive(); wheel values for driving =) 
/* updateWheelSteer(); a 
/* comupte the current actual wheel direction in WheelDirAct[] */ 


if (mode != 100){ 
for(ix =0; ix <ARRAY_SIZE; ix++){ 
/* ere RRC teering/driving interaction® *****#*##*##* — +#/ 
/* here +/- 1/50 of the steering value is added tothe driving  */ 
/* for each specified wheel. Note the negative sign on elements [1] */ 


/* and [3]provide the same direction driving as elements [O] and [2] */ 


Omega_Speed = desiredSpeeds[ix] + 


SteerDriveInteract*desiredAngleRates[ix ]*WheelRadius; /* cm/sec */ 


/* conversion to digits */ 

Speed_Digit = velocityReferenceTable(Omega_Speed,ix) + 
DriveFeedBackGain*(Omega_Speed - actualSpeeds[ix]); 

Steer_Digit = rateReferenceTable(desiredAngleRates[ix]) 


+ steerFeedbackGain*(desiredAngleRates[ix]-actual AngleRates[ix]) 


les2 


50 
a 
52 
3 
54 
55 
56 
a7 
58 
ao 
60 
61 

62 
63 
64 
65 
66 
67 
68 
69 
70 
71 

2 
i 
74 
1 


+ angleFeedbackGain*norm(desiredAngles[ix ]-actualAngles[ix]); 


if (Speed_Digit>DigitsHigh) /* Limitation */ 
Speed_Digit= DigitsHigh; 

if (Steer_Digit>DigitsHigh) 
Steer_Digit= DigitsHigh; 

if (Speed_Digit<DigitsLow) 
Speed_Digit= DigitsLow; 

if (Steer_Digit<DigitsLow) 


Steer_Digit= DigitsLow; 


switch(mode){ 

case 2: 

case 3: 

case 4: 

case 5: 

case 6: 

case 7: 

case 8: 

case 9: 

case 10: 
Speed_Digits[ix]= (short)Speed_Digit; /* casting to short */ 
Steer_Digits[1x]= (short)Steer_Digit; 
break; 


case |: 
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76 speed! = Speed_Digits[ix]; 


77 steer] = Steer_Digits[1x]; 
78 if ( speed1 > 0) speed1--; 
79 if ( speed1 < 0) speed1++; 
80 if ( steerl > O) steer1--; 

8] if ( steerl <Q) steer1++; 

82 Speed_Digits[1x] = speed1; 
83 Steer_Digits[ix] = steer]; 
84 break; 


85 } /* end switch */ 
86 2 ena on / 

Si a end it 7/ 

88 else { 


89 for (x=0; 1x<3; 1x++){ 


90 Steen 121s ix 0 
91 } 

92 for (ix=0; 1x<4; 1x++){ 
93 Speed_Digits[ix] = 0; 
94 } 

95 

96 switch(modeTstate) { 

ae case 0: 

98 Steer_Digits[3] = 50*Flag; 
99 modeT state = 1; 
100 break; 

101 
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102 
103 
104 
105 
106 
107 
108 
109 
110 
11] 
112 
113 
114 
115 


case |: 
modeTstate = 2; 


break; 


case 2: 
modeTstate = 3; 


break; 


case 3: 
modeTstate = 4; 


break; 


case 4: 
modeTstate = 5: 


break; 


case 5: 
modeTstate = 6: 


break: 


case 6: 
modeTstate = 7; 


break: 


case 7: 


iS 


128 modeTstate = 8; 


b> break; 

130 

13] case 8: 

32 modeTstate = 9; 
133 break; 

134 

135 case 9; 

136 modeTstate = 10; 
ey break; 

138 

le case 10: 

140 modeTstate = 11; 
141 break; 

142 

143 case 11: 

144 modeTstate = 12; 
145 break; 

146 

147 case 12: 

148 modeTstate = 13; 
149 break; 

150 

il ease die: 

LZ modeTstate = 14; 
153 break; 
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154 


155 case 14: 

156 modeTstate = 15; 
fo” break; 

158 

159 ease 15; 

160 modeTstate = 16; 
161 break; 

162 

163 case 16: 

164 modeTstate = 17; 
165 break; 

166 

167 case 17: 

168 modeT'state = 18; 
169 break; 

170 

171 case 18: 

172 modeTstate = 19; 
io break; 

174 

is case 19; 

176 if (bitMask&*servoStatus)/* read servo status, */ 
177 { /*wait until] wheel aligned */ 
178 Flag = -Flag; 

179 modeTstate = 20; 


oe, 


180 } 


181 break; 

182 

183 case 20: 

184 Steer_Digits[3] = 0; 
185 modeTstate = 21; 
186 break; 

187 

188 Case 2a8 

189 modeTstate = 22; 
190 break; 

191 

NZ case 22: 

193 modeTstate = 23; 
194 break; 

195 

196 case 23: 

197 modeTstate = 24; 
198 break; 

le 

200 case 24: 

201 modeTstate = 25; 
202 break; 

203 

204 case 2D) 

205 modeTstate = 26; 
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206 break; 


207 

208 case 26: 

209 modeTstate = 27: 
210 break; 

211 

22 ease Ze 

213 modeTstate = 0; 
214 break; 

ei 

216 default : break; 
resi) end Switch */ 


218 i endl else */ 

219 

220 driveSteer(Steer_Digits); 

221 driveSpeed(Speed_Digits); 

222 }/* end driveMotors */ 

223 

224 /* Wheel Driving function */ 

225void driveS peed(short Speed_Digits[]) { 

226 

227 unsigned int *servoControl=(unsigned int *)VWME2170; /* Data Out ay 
228 short *servoOut!=(unsigned short*)(VME9210+0x0082); /* Analog out = */ 
229 short *servoOut3=(unsigned short* )(VME9210+0x0086); /* Analog out test*/ 
230 short *servoOut2=(unsigned short*)(VME9210+0x0084); + /* Analog out test*/ 
231 short *servoOut4=(unsigned short*)(VME9210+0x0088); /* Analog out test*/ 


[39 


on 

233 unsigned int wheelSelect=0x00924924; /* select all wheels for driving or steering */ 
234 

235 *servoControl=wheelSelect; 

236 

237 *servoOut!= (-Speed_Digits[0])<<4; 

238 *servoOut3= (-Speed_Digits[2])<<4; 

239 *servoOut2= Speed_Digits[1] <<4; 

240 *servoOut4= Speed_Digits[3] <<4; 

241 

242 return; 

243} /* driveSpeed */ 

244 

245 

246 

247/* Wheel Steering function */ 

248 void driveSteer(short Steer_Digits[]){ 

249 

250 unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out ah 

251 short *servoOut!=(unsigned short*)(VME9210+0x008A); /* Analog out wheel] */ 
252 short *servoOut3=(unsigned short*)(VME9210+0x008E); /* Analog out wheel3*/ 
253 short *servoOut2=(unsigned short*)(VME9210+0x008C); /* Analog out wheel2*/ 


254 short *servoOut4=(unsigned short*)(VME9210+0x0090); /* Analog out wheel4*/ 
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258 
Eo 
260 
261 
262 
263 
264 
265 
266 
267 
268 


/* select all wheels for driving or turning */ 


unsigned int wheelSelect=0x00924924; 


*servoOut1= Steer_Digits[O|]<<4; /* a neg volt turns wheels clockwise */ 

*servoOut3= Steer_Digits[2]<<4; /* a pos volt turns counter clockwise*/ 

*servoOut2= Steer_Digits[1]<<4; 

*servoOut4= Steer_Digits[3]<<4; 

*servoControl=wheelSelect; /* turn on selected servo motor */ 
return; 


269} /* end of driveSteer */ 


2/0 
Pel 
Ze 


273/* Wheel stop function */ 


274void allStop(){ 


Z1D 
2/6 
Dae 


unsigned int *servoControl=(unsigned int *)WME2170; /* Data Out */ 


/* short *servoOutl=(unsigned short*)(VME92 10+0x0084); */ 


/* deselect all wheels for driving and/or turning */ 


*servoControl=0x00000000; /* turn off selected servo motor */ 
/* *servoOut!= 0.0;*//* temp, does not belong in this function */ 


initBoards(); 


14] 


284 return; 

285 

286} /* end of allStop */ 

287 

288 

289void wheelDrive() 

290 { int 1x,a; 

291 double alpha[ARRAY_SIZE]={30, 0, 0, 0}, betajARRAY_SIZE]={0.0, 0.0, 0.0, 0.0}; 
22 

293 driveMotors(alpha,beta); 

294 

295 return; 

296 }/*end wheelDrive */ 

ey 

298 

299 

300void readDriveEncoders(unsigned long int array[]) 
301 { | 

302 unsigned char *p=(unsigned char*) VMECTRI, cl, c2, c3; 
303 int ix; 

304 long int temp; 

305 


306 for (1x=0; 1x<4; ix++) { /* read all four motors subsequentially */ 


307 
308 = *(p+3)=0x03; /* load output latch from counter */ 
309 *(p+3)=0x01; /* control register, initialize two-bit output latch */ 
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310 

311  /* read three bytes for specific counter ix and save in status */ 
312 /* first access to Output Latch Register reads least significant */ 
oun /* byte first | 

314 

315. cl = *(pt+l) & OxOO0ff; 

316 8 c2=*(p+l) & OxOOff; 

3Y7)— c3 = *(p+1) & OxOOFf; 

318 — array[ix] = ((unsigned int)c1)I ((unsigned int)c2 << 8) | 

319 (unsigned int)c3 << 16); 

320 

321 p=p+4; /* increment pointer for next counter */ 
O22 

B23 

324 } 

2 oeretum; 

326} /* end of readDriveEncoders */ 

B27 

328 

329int readSteerEncoders(unsigned long int array[]) 

330{ 

33] unsigned char *p=(unsigned char*)(VMECTRI + 0x0100), cl, c2, c3; 
Bez int 1x; 
395 

334 


335 for (ix=0; 1x<4; 1x++) { /* read all four motors subsequentially */ 
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336 


351 *(p+3)=0x03; /* load output latch from counter */ 

338 *(p+3)=0x01; /* control register, initialize two-bit output latch */ 
339 

340 


341 /* read three bytes for specific counter 1x and save in status */ 

342 /* first access to Output Latch Register reads least significant byte first */ 
343 

344 scl = *(pt1) & OxOOFf; 

345, c2 = *(pt1) & OxOOff; 

346 =c3=*(ptl) & OxOOff; 

347 —_ array[ix] = ((unsigned int)cl)! ((unsigned int)c2 << 8) | 

348 ((unsigned int)c3 << 16); 

349 

350 

351 = p=p+4; /* increment pointer for next counter */ 
Baz 

2558) 

354 return; 

355} /* end of readSteerEncoders */ 

356 

357 void displayDirections() 

358 { 

359 /*if (edCounter% 10 == 0){ ai 

360 convertBCD(bcdString+9,(unsigned int)(WheelDirDes{2]*RadsToDegrees)); 
361 /*bcdString+9*/ 
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362 


convertBCD(bcdString+9,(unsigned int)edCounter); 


363 —_— bcdString[3]='0"'; 

364 bedString[4]='3'; 

365 __— bcd String[6]='4'; 

366 —_— bed String[7]='0'; 

367 sioOut(0,bcdString); 

368 

369 PaesroOut(O,cirLine); */ /* clear line” 77 
370 

Bel convertBCD(bcdString+9,(unsigned int)(WheelDirAct[2]*RadsToDegrees)); 
372 ~—_—bedString[3]='0'; 

373 ~—s bed String[4]='3’; 

374 ~~ bedString[6]='6'; 

375 __ bedString[7]='0'; 

376 sioOut(0,bcdString); 

S78 

378 } 

o79 

380/* 2 May */ 

381 void displaySpeed() 

382 { 

383 

384 convertBCD(bcdString+9,(unsigned int) 1); 
385 bedString[3]='0'; 

386 bcdString[4]='3'; 

387 bcdString[6]='4'; 
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388 bedString[7]='0'; 

389 sioOut(0,bcdString); 

390 

39] /* sioOut(O,clrLine); */ /* clear line = */ 

392 

393 convertBCD(bcdString+9,(unsigned int)steerReadings[1 ]); 
394 bedString[3]='0'; 

Bo» bedString[4]='3'; 

396 bedString[6]='6'; 

397 bcdString[7]='0'; 

398 sioOut(0,bcdString); 

oo) 

400 

401 void testDrivel() 

402 { 

403 desiredAngleRates[O] = 1; 

404 desiredAngleRates[1] = 1; 

405 desiredAngleRates[2] = 1; 

406 desiredAngleRates[3] = 1; 

407 desiredSpeeds[0O] = 0; /* wheels 2&4 must have minus sign */ 
408 desiredSpeeds[1] = 0; /* wheels 2&4 must have minus sign */ 
409 desiredSpeeds[{2] = 0; /* wheels 2&4 must have minus sign */ 
410 desiredSpeeds[3] = 0; /* wheels 2&4 must have minus sign */ 
All 

412} 

413 
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414/* 2 May */ 

415void testDrive() 

416{ 

417 double MM, RES, N= 1044548, C=0.001; 
418 

419sioOut(0,"Entering testDrive ...\n\r"); 
420 

421 

422if(20<(N - steerReadings[1])*C){ 
423 

424 MM=20.0; 

Bes} 

426 else { 

427 

428 MM=(N - steerReadings[1])*C; 
429 } 

430 

43] 

432 if(MM>0){ 

433 

434 RES=MM; 

435 } 

436 else{ 

437 

438 RES=0.0; 

439 } 
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440 
44] 
442 


443 /* RES=max(min(20,(N - steerReadings[0])*C),0); */ 


4s 


445sioOut(0,"Leaving testDrive ...\n\r"); 


446 desiredSpeeds[1] = -(RES); /* wheels 2&4 must have minus sign */ 


447 

448 return; 

449} 

450 

451 

452 

453 

454 

455void compute ActualRates() 
456{ 

457 

458int 1; 

459 double count,speed; 
460 

461 forGi=0; 1i<=3; 1++) 
462 { 


463 if(PreviousCountSpeed[i] == 99999999) /* for derivative for speed */ 


464 actualSpeeds[iJ= 0.0; 
465 else 
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466 actualSpeeds[i]= 

467 (convertDifference((driveReadings[i] - PreviousCountSpeed[i])) 
468 *DigitloCmDrive[i])/DeltaT; 

469 PreviousCountSpeed|[i] = driveReadings[1]; 

470 

471 if(PreviousCountSteer[i] == 99999999) /* for derivative for steering */ 
472 actualAngleRates[i]= 0.0; 

473 else 

474 actualAngleRates{i]= 

475 (convertDifference((steerReadings[i] - PreviousCountSteer[1])) 
476 *digitfoRadSteer)/DeltaT; 

477 PreviousCountSteer[i] = steerReadings[1]; 

478 } 

479} 

480 

481 

482 

483 void accumulateDriveSpeed() 

484 { 

485 int 1; 

486 

487 for(i=0;1<=3;1++) { 

488 Display_Speeds[i] += actualSpeeds[1]; 

489 } 

490 return; 


491 } 
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492 

493 void accumulateDriveSteer() 

494 { 

495 int1; 

496 

497 for(i=0;1<=3;1++) { 

498 Display_Steers[{i] += 10*actualAngleRates[1]; 
499 actualAngles[i] += actualAngleRates[i]*DeltaT; 
500} 

501 return; 

502 } 

503 

504/* added 15 may */ 

SO5void displayDriveAngle() 

506 { 

507 

508 double angle, anglel, angle2, angle3; 

509 angle = actualAngleRates[0] * 1000.0; 

510 anglel = actualAngleRates[{1] * 1000.0; 

S11 angle2 = actualAngleRates[2] * 1000.0; 

512 angle3 = actualAngleRates[{3] * 1000.0; 

513 

514 if (edCounter%100 == 0){ 

515 convertInt(bcdString+9,(int)desiredAngleRates[0]); 
516 bcedString[3]='0'; 

Dlg bedString[4]='3'; 
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518 
Se 
520 
oe 
577 
523 
524 
2) 
526 
27 
525 
20 
530 


OZ 
p35 
534 
535 
536 
ey 
538 
oe 
540 
54] 
542 
543 


bcdString[6]='4'; 
bcdString[7]='0'; 
sioOut(0,bcdString); 


convertInt(bcdString+9,(int) angle); 


bedString[3]='0'; 
bedString[4]='3'; 
bcdString[6]='6'; 
bcdString[7]='0'; 
sioOut(0,bcdString); 


convertInt(bcdString+9,(int) angle1); 


bcdString[3]='0'; 
bcdString[4]='4'; 
bcdString[6]='6'; 
bcdString[7]='0'; 
sioOut(O,bcdString); 


convertiInt(bcdString+9,(int) angle2); 


bcdString[3]='0’; 
bedString[4]='S’; 
bcdString[6]='6'; 
bcdString[7]='0'; 
sioOut(0,bcdString); 


convertInt(bcdString+9,(int) angle3); 


bcdString[3]='0'; 
bedString[4]='6'; 
bedString[6]='6'; 
bedString[7]='0'; 


lel 


544 sioOut(0,bcdString); } 

545 

546 return; 

547} 

548 

549 

550 

551double velocityReferenceTable(double desired Velocity,int 1) 
33 

553 double inVelocity, 

554 outVelocity; 

5 

556 inVelocity=new_abs(desired Velocity); 
557 

558 if (inVelocity>=0.0 && inVelocity<=5.0) 
559 — out Velocity = inVelocity*K1]1 [i]; 

560 

561 if GinVelocity>5.0 && inVelocity< 8.0) 
562  outVelocity = inVelocity*K2[i]; 

563 

564 if (inVelocity>=8.0 && inVelocity<20.0) 
565 out Velocity = inVelocity*K3[i]; 

566 

567 if (inVelocity>=20.0 && inVelocity<= 70.0) 
568  outVelocity = inVelocity*K4{[i]; 

569 
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570 if (inVelocity>70.0 && inVelocity<K5) 
571  ~outVelocity = inVelocity*K6[i]; 

S72 

573 if (inVelocity> KS) 

574  outVelocity=1023; 

ae 

576 if (desired Velocity< 0.0) 

577 ~— out Velocity = - out Velocity; 

578 

579 return outVelocity; 

580} /* end velocityLookupTable */ 

581 

582 

583double rateReferenceTable(double desiredRate) 
584 { 

585 double inRate, 

586 outDigit; 

o7 

588 /*outDigit = new_abs(desiredRate); *//* test only */ 
589 

590 inRate=new_abs(desiredRate); 

59] 

592 if (inRate<= 5.234) 

593 ~=—s out Digit = inRate* 195.4155 ; 

594 else 


a) outDigit=1023; 


Iie 


596 

7 

598 if (desiredRate< 0.0) 

599 ~=—s out Digit = - outDigit; 

600 

601 return outDigit; 

602 } 

603 

604 

605 

6.06 232A GG OG GOCE ICGG  ICA AG  A A R 
607 Function convertDifference() returns the difference between the new shaft 
608 encoder position and the old shaft encoder position. The shaft encoder values 
609 contain only 24 bits (0Ox000000-Oxffffff). The routine adjusts for the trans- 
610 ition from Oxffffff to OxO00000 and vice versa. 

6 |] 222 EO OR OR EK CCR CO FO ACEC A CO IC COCO COR EO OCR OC COOK OK 7 
612 

613int convertDifference(int value) 

614{ 

615 if(value < -0x800000) 

616 value &= OxOOffffff; 

617 else if(value >= 0x800000) 

618 value l= OxffOO00000; 

619 

620 return value; 


621} 
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623/* ------------------------------------------------- = 2 $= === 22222 = = a 
624 * : 
OZ) File: SERVO.C - 
626 * 2 
627 * Environment: GCC Compiler v2.7.2 : 
628 * Last update: 30 January 1997 : 
629 * Name: Thorsten Leonardy : 
630 * Purpose: — Provides the kernel for SHEPHERD. ? 
631 * - 
632 * Compiled: >gcc -c -m68040 -o servo.o servo.c . 
33, * - 
634 * --------------------------------------------------------------------------- a 
635 

636 

637 

638/* ------------------------------------------------------------------------- = 
639 * readWheelStatus() : 
640 * e 
641 * Environment: GCC Compiler v2.7.2 % 
642 * Last update: 20 February 1997 = 
643 * Name: Thorsten Leonardy i 
644 * Purpose: This function reads the wheels counter status. ss 
645 * This routine makes use of the fact that arrays are stored * 
646 * in memory consecutively. * 
647 * - 
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648 * array points to the beginning of the array 'wheelEncoder’. = 


650void readWheelStatus(unsigned char *array) 
651 { 

652 unsigned char *p=(unsigned char*)VMECTRI; 
653 int ix; 

654 


655 for (ix=0; 1x<8; 1x++) { /* read all eight motors subsequentially */ 


656 

657 = *(p+3)=0x03; /* load output latch from counter */ 

658 *(p+3)=0x01; /* control register, initialize two-bit output latch */ 
659 


660 /* read three bytes for specific counter 1x and save in status */ 

661 /* first access to Output Latch Register reads least significant byte first */ 
662 *(array+3)=*(p+1); 

663 = *(array+2)=*(p+1); 

664 *(array+1)=*(p+l1); 

665 *(array+0)=0; 


666 

667 array+=4; /* point to next entry in wheelEncoder*/ 
668  p=p+4; /* increment pointer for next counter */ 
669 


6/70 if (ix==3) p=(unsigned char*)VWMECTR2; /* access the second VME Counter */ 


67 | 
672 } 
673 return: 
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674} /* end of readWhee!l Status */ 


675 

676 

677/* ------------------------------------------------------------------------- - 
678 * clearShaftEncoder(unsigned short motors) . 
oro * if 
680 * Environment: GCC Compiler v2.7.2 ss 
681 * Last update: 04 March 1997 ss 
682 * Name: Thorsten Leonardy ae 


683 * Purpose: This function clears the selected shaft encoder. * 


684 * * 
685 * motors bit mask to select motors, eg. Ox042 selects motor 2 and 7 * 
686 * to be cleared. : 
Een 8 8 eee * / 


688 void clearShaftEncoder(unsigned short motors) 

689 { 

690 unsigned char *p=(unsigned char*)VMECTRI1; 

691 int 1x; 

692 

693 for (1x=0; 1x<8; ix++,motors/=2) { 

694 if (motors & 0x01) *(p+3)=0x04; /* clear respective counter */ 
695 p=p+4; /* access next pointer */ 

696 if (ix==3) p=(unsigned char*)VMECTR2; /* access the second VME Counter */ 
697 } 

698 return; 


699} /* end of clearShaftEncoder */ 


I> / 


701 

702/* -------------------------------------------- = $22 $= 22 noone nn nnn nn = f= 
703 * align() ss 
704 * Environment: GCC Compiler : 
705 * Last update: 07 August 1997 m it 


706 * Name: Thorsten Leonardy , Yutaka Kanayama, Ed Mays * 
707 * Purpose: ‘This function will align SHEPHERD's wheels such that all * 


708 * will point in the forward direction. It utilizes the hall * 

P09 sensors for each of the four wheels. Crucial parameters * 

HAO > are as follows: : 
ne * 
712 * ------------------------------------------------------------------------- a 


713 void align(void) 

714{ 

715 unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 

716 unsigned short *servoOut=(unsigned short*)(VME9210+0x008A);  /* Analog out */ 
717 unsigned short *servoStatus=(unsigned short *)(VME9421+0x00ca); /* digital input */ 
718 unsigned short bitMask=0x8000, bitMask]; — /* access bit 15 for align wheel 1 */ 
719 unsigned int wheelSelect=0x00004000; /* select servo for turning wheel | */ 


720 int ix, notYet; /* just a counter */ 


i22 “don 
723 ~=notYet=0: 
724 ~~ bitMask1 = bitMask; 


725 for (ix=0; ix < 4; 1x++) 
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726 { 


p27 if (bitMask1 & *servoStatus) 
728 { 

#29 Steer_Digits[1x] = 0; 

730 } 

731 else 

732 { 

733 Steer_Digits[1x] = 40; 

734 not Yet++; 

1S } 

736 bitMask1 = bitMask1 >> 1; ~—/* select next status align bit */ 
fo } 


738  driveSteer(Steer_Digits); 

739 ~—=—+} ~ while(notYet); 

740 *servoControl=0x00000000; /* disable all wheels i 
741 return; 


742} /* end of align */ 


743 

744/* --------------------------------------------------------22 22-2222 2 222 2--- - 
745 * alignAfterRotate() x 
746 * Environment: GCC Compiler * 
747 * Last update: 07 August 1997 m a 


748 * Name: Thorsten Leonardy, Yutaka Kanayama, and Ed Mays * 
749 * Purpose: = This function will align SHEPHERD's wheels such that all * 
50) * will point in the forward direction. It utilizes the hall a 


(2 ae sensors for each of the four wheels. Crucial parameters * 


ok, 


va2 > are as follows: = 


755void alignAfterRotate(void) 

756 { 

757 unsigned int *servoControl=(unsigned int *)VME2170; /* Data Out */ 

758 unsigned short *servoOut=(unsigned short*)(VME9210+0x008A); /* Analog out */ 
759 unsigned short *servoStatus=(unsigned short *)(WME9421+0x00ca); /* digital input */ 
760 unsigned short bitMask=0x8000, bitMask1; = /* access bit 15 for align wheel 1 */ 
761 unsigned int wheelSelect=0x00004000; /* select servo for turning wheel | */ 

762 int ix, notYet; /* just a counter */ 

763 

764 do { 

765 notYet =0; 

766 ~~ bitMaskI = bitMask; 


767 ~~ for (ix=0; 1x < 4; 1x++) 


768 { 

769 if (bitMask1 & *servoStatus) 

770 { 

771 Steer_Digits[ix] = 0; 

fae } 

773 else 

774 { 

oD if( 1x==1 |) ix==2 ) 

776 Steer_Digits[ix] = 40; /* for wheel | and 2, rotate CCW */ 
777 else 
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7718 Steer_Digits[ix] = -40; /* for wheel 0 and 3, rotate CW */ 


779 not Yet++; 

780 } 

781 bitMask1 = bitMask] >> 1;  ~—/* select next status align bit */ 
782 } 


783 ~~ driveSteer(Steer_Digits); 

784 } while(notYet); 

785 *servoControl=0x00000000; /* disable all wheels * / 
786 return; 

787} /* end of align */ 

788 


791 * alignWheels() ‘i 

792 * Environment: GCC Compiler Zn 
793 * Last update: 07 January 1997 . 
794 * Name: Thorsten Leonardy a 


795 * Purpose: This function will align SHEPHERD's wheels such that all * 


fOGe* will point in the forward direction. It utilizes the hal] * 
7 sensors for each of the four wheels. Crucial parameters * 
798 * are as follows: * 

foo . 


800 * servoContro] Base address for the channels controling the servo motors * 


801 * switch servos on an off by accessing this address. = 
802 * Each servo is controlled by three bits: 
803 * bits 0..2 -> driving wheel 1 ° 


161 


804 * 3..5 -> driving wheel 2 7 


805 * 6..8 -> driving wheel 3 e 
806 * 9..11 -> driving wheel 4 
807 * 12..14 -> turing wheel 1 _ 
808 * 15..17 -> turning wheel 2 2 
SUo 4 18..20 -> turning wheel 3 ei 
810 * 21..23 -> turning wheel 4 i 
Sil? 24..31 -> not used ss 
Si x 


813 *servoOut Base address for the analog output card controlling the * 


814 * speed of the servos. Only the highest 12 bits are used. * 
a * Ox0010 -> selects lowest positive velocity = 

816 * Ox7ff0 -> selects highest positive velocity t 

817 * OxfffO -> selects lowest negative velocity (1.e-1 m/s) * 
818 * Ox8000 -> selects highest negative velocity (1.e. -]000m/s) * 
O19 - 

820 * It has been found that the MSB does not work properly. * 
ole Therefore, the velocities should lie within 11 bit range, * 
822 * -1024 <= velocity <= +1023 t 

ooo. a 

824 * servoStatus Base address for reading the servo status = 
S25 > The alignment bits are: Port B, Bit 15 for wheel 1 = 
S26." Bit 14 for wheel 2 - 

Ba Bit 13 for wheel 3 ss 

828 * Bit 12 for wheel 4 a 

ooo = ss 
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S32 

833 void alignWheels(void) 

834 { 

835 unsigned int *servoControl=(unsigned int *)VWME2170; /* Data Out */ 

836 unsigned short *servoOut=(unsigned short*)(VME9210+0x008A); _/* Analog out */ 
837 unsigned short *servoStatus=(unsigned short *)(VME9421+0x00ca); /* digital input */ 
838 

839 unsigned short bitMask=0x8000; = /* access bit 15 for align wheel 1 */ 

840 unsigned int wheelSelect=0x00004000; /* select servo for turning wheel | */ 

841 int wheel; /* just a counter */ 

842 

843 /* -------------------------------------------------------------- é 

844 * align wheels subsequentially, start with wheel 1 (front right) * 

845 * -------------------------------------- 2222-22-22 = 2222 -------- yy 


846 for (wheel=1; wheel<5; wheel++) { 


847 

848 *servoOut++=0x0200; /* set output value for servo first a 
849 /* 0x0010 corresponds to smallest velocity  */ 

850 *servoControl=wheelSelect; /* turn on selected servo motor a 


85] while(!(bitMask&*servoStatus)); /* read servo status, wait until wheel aligned */ 


852 wheelSelect= wheelSelect<<3; = /* select next servo (motor) 7 
853 bitMask = bitMask >> 1; /* select next status align bit a 
854 } 

855 
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856 *servoControl=0x00000000; /* disable all wheels x] 


857 

858 /* clearShaftEncoder(OxOff); */ /* clear all shaft encoders 7 
859 

860 /* sioOut(O,"aligned ...");  */ /* Output Message z/ 
861 return; 


862} /* end of alignWheels */ 
863 
864 
865 
866 


[PRE Ae A eR OKO A A ee ee 2 2 RR 2 ie ee oie ie 2k 2 2 Ke OR 2 ie oie 2c oie ie fe 2g ie oie Ae Ae oe 2 2 2 2 2k 2k 2k 2 fe 2 2 2 2k 2k 2c 2 2c i 2k 2k oi ok ok 


867 End of servo.c 
868 


Re ee ES EE ee ee ee ee ee EE ee oe a Ee 


869 
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APPENDIX F: SOURCE CODE (TIMER.C) 


tae enn ee OUMEAOU N82 22 ee ee ** 

x * 

= Tiles TIMER .C ss 

x + 

* Environment: GCC Compiler v2.7.2 . 

* Last update: 29 January 1997 : 

* Name: Thorsten Leonardy si 


* Purpose: — Provides routines related to the AM9513 Timer Circuit, such * 


bs as interrupt initialization, 3 
* Compiled: >gcc -c -m68040 -o timer.o timer.c a 
* * 
<META pl a TC, * / 


#include "shepherd.h" 


#include "timer.h" 


void timerStart(void) 


ong *vadr; 

unsigned char *p; 

short *ctrlPort = (short*) TIMER_CTRL; 
short *dataPort = (short*) TIMER_DATA; 


/* initialize the interrupt counter */ 


intCounter=0; 


165 


29 
30 
31 
32 
33 
34 
35 
36 
37 
38 
39 
40 
41 
42 
43 
44 
45 
46 
47 
48 
49 
50 
5] 
52 
53 
54 
55 
56 
57 


/* load address for interrupt service routine */ 
vadr=(long*) VBA_TIMER; 


*vadr=(long) TimerHandler; 


/* Issue commands to set control and data register a 
/* refer to Fig 1-20, 1-8, 1-12 a 
*ctrlPort=Oxffff; /* Master reset, clear data registers */ 
*ctrlPort=Oxff5f; /* load all counters * 
*ctrlPort=Ox ffef; /* Set MM13 (Enter 16-bit bus mode) */ 
*ctrlPort=Oxff17; /* Select master mode register a 
*dataPort=0xale0; /* set master mode register ... */ 
ie +------------ > f==1secinterruptinterval */ 

fe e=—10) 1 Sec ey 

/* di== 0-0 lFsec +] 

i C—— 0.0 lesec a 
*ctrlPort=O0xff05; /* Select CMR timer 5 ... a 


/* utilize Data Pointer Sequencing */ 


*dataPort=0x0e32; /* and write to counter mode register */ 
/* +------------- = f = 10000 ay 

/* multiply value according to dataPort below e= 1000 | 
/* by the factor set here to obtain timing... d= 100 oy, 

is Ca | 

ifs b= al | 
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58 
59 
60 
61 
62 
63 
64 
65 
66 
67 
68 
69 
70 
71 
1p 
73 
74 
75 
16 
77 
78 
79 
80 
8] 
82 
83 
84 
85 
86 


/* to obtain the correct timing, multiply value determined in data-*/ 
/* port below by the factor given above. E.g. dataPort is set to */ 


/* 58 (corresponding to 1Qusec) and factor 1000 is chosen above, */ 


/* then the interrupt would occur every 10msec! my 

*dataPort=58; /* load register, 58 -> 1Ousec | 
ie 930 -> Imsec a 

*ctrlPort=Oxff70; /* load and arm timer 5 a) 


p=(unsigned char*)ISM_TIMER; /* ISM Configuration for Timer a 
*p=Oxcb; /* assert LIRQ-3 to VIC / 


p=(unsigned char*)VIC_LIRQ3; /* VIC LICR for LIRQ-3 from ISM —*/ 
*p=0x03; /* assert IRQ-3 from VIC to 68040 = */ 


return; 


} /* end of timerStart */ 


[8 8 FB 8 2 EE A eee Ce ie 2 2 ee 22 2 22 2 2 2k 2 ee ek ie 2g ie 2c ie ke 2k 2 2 2 2 2 2 ee ee ek 2 ok OK 2 ke 2k 2 


Assembler routines 


Boe eR eget ety te Reet eR eRe Ra Re Ae Aa i rR Ae or oh OR Oe oe ee 


/* TimerHandler, its address is set from within timerStart */ 


asm(" 
even 


text 
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87 glob] _TimerHandler 


88 

89 _TimerHandler: 

90 

9] 

a2 link a6,#-184 /* alocate 184 Bytes on stack to save registers */ 

93 fsave a6@(-184) 

94 fmovemx fp0-fp7,sp@- = /* move floating point registers 80 bit each +) 
OS fmovel fpcr,sp@- /* move floating point Control Regioster 7 
96 fmovel fpsr,sp@- /* move floating point status register | 

97 fmovel fpiar,sp@- /* move floating point Instruction address register */ 
98 moveml d0-d7/a0-a5,sp@- /* save data and address registers (14*4 Byte)  */ 
oD 

100 

101 addq.| #0x01,_intCounter /* increment interrupt counter a7 
102 move.w #Oxffe5,0xfff41002 /* clear toggle out for timer 5 ba 
103 

104 move.l #Oxffff0081,al /* load VME9421 Status register uy 
105 eor.b #0x02,(al) /* toggle green indicator light to indicate timer */ 
106 /* for interrupt handling 1s working properly... */ 

107 and.b #Oxfe,(al) /* turn red light on to indicate that motion control*/ 
108 /* will start (this will assert the SYSFAIL line on */ 

109 /* the VME-Bus, but we don't care at this point). */ 

110 

111 jst driver /* execute motion control part */ 

112 

113 


114 move.| #Oxffff0081,al /* load VME9421 (digital out board) Status register */ 


aS or.b #0x01,(al) /* turn off red indicator light to indicate that */ 
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116 
117 
118 
119 
120 
12] 
122 
123 
124 
5 
126 
127 
128 
129 
130 
il 
132 
133 


/* motion control is done. =f 


moveml sp@+,d0-d7/a0-a5 
fmovel sp@+,fpiar 
fmovel sp@+,fpsr 

fmovel sp@+,fpcr 
fmovemx sp@-+,fp0-fp7 
frestore a6 @(-184) 

unlk a6 


FORO GOR CCC CCC CR OCI CICK aK 


End of timer.c 


re ena te ack ee he at Pa ama tear tec hectic eh cha chek Teche he teat nie creat cee tt ee et et, 
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APPENDIX G: SOURCE CODE (MATH.C) 


The following code was modified by: Professor Kanayama, Thorsten Leonardy, 


Edward Mays, and Ferdinand A. Reid. 


ome Pile: MATH .C 

4 x 

5 * Environment: GCC Compiler v2.7.2 
6 * Last update: 17 March 1997 

7 ™* Name: Thorsten Leonardy 


8 * Purpose: A Simple Math library. 


11 #include "shepherd.h" 

12 #include "math.h" 

13 

14 #define pio4 0.785398163 
15 #define pio2 1.570796327 
16 #define pi 3.141592654 
17 #define pi2 6.283 185307 


18 


17] 


12 

20 

9) |] PERRI ICI II IOI I A ICSC A AG CICA ACI AG A 1 aK a ok ak 21 ok 
22 FUNCTION: norm() 

23 PARAMETERS: double angle — ---- the angle to normalize 

24 PURPOSE: normalize the input angle between -PI and PI 

25 RETURNS: double: the normalized angle in radians 

26 COMMENTS: This is the most common normalizing function used in the system 
De This performs that same as norm() and normalize)() in MML1O0. 

2.8 FEA CIC A ICICI IA A IOI I I I ACR I ACC ICR AR ICR oR OR IC CR OK CH / 
29 double norm(double angle) 

30 { 

31 while ((angle > pi) Il (angle <= -pi)) 

oe 

33 if (angle > pi) 

34 angle -= pi2; 

35 else 

36 angle += pi2; 

Sey 

38 return angle; 

59} 


40 
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46 * new_abs() 

47 * 

48 * Environment: GCC Compiler v2.7.2 

49 * Last update: 14 March 1997(mod 2 April 97 by Ed Mays) 
SO * Name: Thorsten Leonardy 

51 * Purpose: A function returning the absolute value of x. 
Sr eck ee ee 
53 double new_abs(double x) 

54 { 

Sein (x>=0.0) 

56 return (x); 

vee cise 

58 return(-x); 

ao} 

60 

61 


62 
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64 /* ------------------------------------------------------------------------- = 

65 * atan2() _ 
BO. i 

67 * Environment: GCC Compiler v2.7.2 si 

68 * Last update: 17 March 1997 7 

69 * Name: Thorsten Leonardy = 

70 * Purpose: Computes tan(y/x) where x,y are real. If both variables are * 
Tie Zero, atan2 returns zero. For any other values, atan2 will * 
et return the positive angle for the (x,y)-pair, e.g., - 
[ors (x,y)=(0,-1) would return atan2=3/2*pi ! - 
dst 1x determines the accuracy (highest order term in expansion)* 
12% For the worst case, ly/x| close to one, 1x should be very * 
Tis high. Here is some data: Gi 

Fit Ix/yl 1x accuracy of result [rad] si 

157 0.9 101 +- 1.88*10E-7 - 

(ors 1001 +- 1.57*10E-49 - 

S0me OOO re 10) +- 3.45*10E-3 z 

Slave 1001 +- 4.18*10E-8 = 

S207 10001 = +- 2.18 * 1OE-48 * 

Saas 0.999 101 +- 8.76* 10E-3 5 

84 * 1001 +- 3.65*10E-4 = 
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Gea 10001 = +- 4.50* 10E-9 F 


87 double atan2(double y, double x) 

88 { 

89 double erg=0.0, z=0.0, z2; 

90 int 1x=101, flag1=0,flag2=0; 

) 

92  1f ((new_abs(y)>new_abs(x))&&(y!=0)) 

mo { 

94 ay. /* in case ly/xl>1 compute atan(1/z) */ 
95 flagl=(y>0)-(y<0); /* a handy sign-function */ 

DG. } 

o7 seeise if (x!=0) 

98 { 

oo NX: /* in case ly/xl<1 compute atan(z) */ 
100 flag2=(x<0.0); /* in this case need to add pi to final result */ 
101} 

102 

103 /* From here on Izl must always be less than one !!! */ 
Hee Z2=7"7; 

105 


106 /* Taylor expansion */ 
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107 if (new_abs(z)<1.0) { /* computation for ly/xl<1 */ 

108 while (1x>1) { 

109 erg=z2*(1.0/ix-erg); /* try alternatively for accuracy: (z2/ix)*(1.0-ix*erg) */ 
110 ike— 

Teel } 

Nee erg=z-z*erg; 

eS eeeet 

114 else erg=((z>0.0)-(z<0.0))*pio4; /* for ly/xl=1] result is either +- pi/4 */ 

is 

116 if (flagl==1) erg=pio2-erg; /* point lies in 3rd or 4th octant for flagl=+1 */ 
117 else if (flagl==-1) erg=-pio2-erg; /* ... or in 6th or 7th octant for flagl=-1  */ 
118 if (flag2) erg=erg+pi; /* point lies in 4th or 5th octant ai 

119 /* if (erg<0.0) erg=erg+pi2; deleted 6/27/97 e 

120 

121  return(erg); 

ee} 

123 

124 

125/* ------------------------------------------- oon enn nn nnn en nnnnnnn=------ ‘i 

126 * atan() yk + 

LD ana ay 


128double atan(double x) 
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9 { 
130 
Ie I 
sy 
3 
134 
15 
136 
Ss 
138 
ee 
140 
14] 
142 


143 


145 
146 
147 
148 
149 


150 


double erg=0.0, z=0.0, z2; 


int ix= 101, flag=0; 


neo =— 0:0) 
return (0.0); 
if (new_abs(new_abs(x)-1.0) < 0.00001 ) 
return (pio4 * x); = /* return +- pi/4 i 
if (new_abs(x) > 1.0) 
ZI OL. /* in case Ixl>1 compute atan(1/x) */ 
flag=(x>0)-(x<0);  /* a handy sign-function */ 


} 


else 
Z=X; /* in case Ixl<1 compute atan(x) */ 
Bo=7 72; /* From here on Izl is less than one !!! */ 


/* Taylor expansion */ 
while (ix>1) 
erg=z2*(1.0/1x-erg); 


1X-=2; 


ue 


i leicie—=2Z-2 ce, 

152 if (flag == 1) erg = pio2-erg; 
153 if (flag == -1) erg =-pio2-erg; 
154 return(erg); 

eo 
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160 * cos() - 

161 * # 

162 * Environment: GCC Compiler v2.7.2 is 
163 * Last update: 17 March 1997 a 

164 * Name: Thorsten Leonardy si 

165 * Purpose: | Computes cos(x) where x can be any real number. 


166 * ix determines the accuracy (highest order term in expansion)* 


168double cos(double x) 
169 { 
170 double erg; 


171 int quadrant, ix=20; /* ix must be an even number ay 


yz 
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173 /* analyze and reduce x to the appropriate range ... */ 


174 quadrant=(x/pio2+(x>=0)-(x<0))/2; /* determine in what sector xis */ 


175. x=x-quadrant*pi; /* reduce x to region [-pi/2...p1/2]*/ 
BG) X=X*x; /* compute x2 and store in x = 

lie eere—1.0: 

178 


179 /* the cosine taylor computation is a one-liner ;-) */ 
180 while (ix>0) { 

181 erg=1.0-erg*x/ix/(ix-1); 

182 = 1x-=2; 

ss } 

184 

185 /* shift sign if quadrant is not 1,3,5,... */ 
186 if (quadrant%2) erg=-erg; 

187 

188 return(erg); 

189} 

190 


7) 


193 * sin() * 


194 * * 
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195 * Environment: GCC Compiler v2.7.2 a 
196 * Last update: 14 March 1997 : 
197 * Name: Thorsten Leonardy = 


198 * Purpose: | Computes sin(x) where x can be any real number. * 


200double sin(double x) 

201{ 

202 return(cos(x-pio2)); /* since sin(x)=cos(x-pi/2) */ 
203} 

204 

205 


206 


208 * sqrt() : 
209 * Ed Mays and Ferdinand Reid March 1997 e 


210 * Environment: GCC Compiler v2.7.2 - 


212double new_sqrt(double x) 
Ze 

214 double x1, x2; 

215 int count; 


216 
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217 

218 if (x == 1.0) return(1.0); 
el | .0; 

220 for (count=0; count < 10; count++){ 
pele X2=.5 * (xl + x/x1); 
eee Xi = x2; 

y23 } 

224 

72>) return (x2); 

226 } 

po] 

228 

220 


JAY) 


232 * new_saqrt!() 
233 * Ed Mays and Kanayama 


234 * Environment: GCC Compiler v2.7.2 


236double new_sqrt!(double x) 
ea7 { 


238 double x1, x2; 


18] 


239° x! =e: 

240) x2= 120: 

241 while(new_abs(x1-x2) < 1.0e-9) 
242 { 

243 XJ =a15 

244 Melee oe Ce KK 
245} 

246 

247 return (x1); 

248 } 

249 

250 

Zi 

250 

253/* ed move to math.c*/ 
254double min (double a, double b) 
2554 

Zea (a= b) 

257 return a; 

258 else 

25) return): 


260} 
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261 

262/* ed move to math.c*/ 
263double max (double a, double b) 
264 { 

265 if (a>=b) 

266 return a; 

267 else 

268 return b; 

269} 

ee) ed*/ 

271 

Bad 

2713 

274 

BS 

210 


277 


[757 ORICA II IK ak 


2/78 End of math.c 
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APPENDIX H: SOURCE CODE (UTILS.C) 


10 


1] 


IZ 


FIT Gone * 
‘ * 
*FILE: UTILS .C * 
a * 
* ENVIRONMENT: GCC COMPILER V2.7.2 * 

* LAST UPDATE: 03 FEBRUARY 1997 * 
*NAME:  THORSTEN LEONARDY * 


* PURPOSE: PROVIDES THE UTILITY FUNCTIONS FOR 


PROGRAM SHEPHERD. ‘ 


* * 


* COMPILED: >GCC -C -M68040 -O UTILS.O UTILS.C * 


#INCLUDE "SHEPHERD.H" 
#INCLUDE "UTILS.H" 


#INCLUDE "MATH.H" 


UNSIGNED INT PIFLAG=0; 


UNSIGNED INT MAGIC=0X 1237; 
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21 EXTERN CHAR JOYSTICK[]; /* DEFINED IN SHEPHERD.C */ 


22 EXTERN CHAR BCDSTRING[]; /* DEFINED IN SHEPHERD.C */ 


23 
24 
Da i ce ae Se ean Es 2 * 
26 *READCLOCK( * 
27 —* * 
28 * ENVIRONMENT: GCC COMPILER V2.7.22 
. 
29 *LAST UPDATE: 26 FEBRUARY 1997 * 
30 *NAME: | THORSTEN LEONARDY * 


31 *PURPOSE: THIS FUNCTION READS THE VALUES FROM THE 
CALENDAR CLOCK * 


32 ee DEVICE MTK48T08 (SEE OMNIBYTE HANDOUT CHAP. 
2.9.4)INTO * 

i al GLOBAL VARIABLE CLOCK. THE FORMAT IN CLOCK 
IN DECIMAL IS: * 

Baga *k 

oD ace CLOCK = YYMMDDHHMMSS ; 

36 * * 

37°—~CS*~S ILE TO RETRIEVE THE DATE PERFORM 


DATE=CLOCK/1000000; i 


Soe TO RETRIEVE THE TIME PERFORM 
TIME=CLOCK% 1 000000; ii 


39 * * 


40 * CALLED BY: FUNCTION TIMERHANDLER IN ‘TIMER.C 
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aie 


D ?.... ee ee ____ */ 

43 

44 

45 

46 

47 

FA i |e eee tac nnn sa * 

49 * PITEST() * 

50. * * 

51 * ENVIRONMENT: GCC COMPILER’ v2.7.2 
: 

52 *LAST UPDATE: 24 FEBRUARY 1997 * 

53 *NAME: THORSTEN LEONARDY * 

54. * PURPOSE: THIS FUNCTION TESTS INTERPROCESSOR 
SIGANNLING VIA PI-46* 

55 * INTERRUPT. * 

DON) 8 -2aa----—---- s */ 


57 VOID PITEST(VOID) 
58 { 

59 LONG *VADR: 

60 | UNSIGNED CHAR *P: 
61 


62 /* SET ADDRESS FOR PROCESSOR INTERRUPT HANDLER 
ROUTINE */ 
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63 VADR=(UNSIGNED LONG *)VBA_PI; 

64 *V ADR=(UNSIGNED LONG)PIHANDLER; 
65 

66 P=(UNSIGNED CHAR *)ISM_PI, 


67. *P=(UNSIGNED CHAR)OXEO; /* SET IP-ISM TO 68040 ON 
LIRQ-6 */ 


68 
69 P=(UNSIGNED CHAR *)VIC_LIRQ6; 


70 *P=(UNSIGNED CHAR)0X06; /* CONFIGURE VIC068 LIRQ-6 
“ 


a 


Ap: P=(UNSIGNED CHAR *)APP_ICR; /* ABORT/PROC 
INTERRUPT CTRL */ 


is *P = *P | (UNSIGNED CHAR)OX02; /* ASSERT IP-46 INTERRUPT 
| 


74 


75 WHILE(PIFLAG==0) { INTCOUNTER=0; } /* WAIT FOR PI 
INTERRUPT */ 


76 
Wg IF (PIFLAG==MAGIC) { 


78 /* ‘TOGGLEVME((UNSIGNED CHAR *)VME9210,0X02); */ 


a? SIOOUT(O,"PASSED"); 
80 } 
8] Ee 


82 /* TOGGLEVME((UNSIGNED CHAR *)VME9421,0X02); */ 


83 SIOOUT(0,"FAILED"); 


188 


84 } 
85 
86 RETURN; 


87 } /* END OF PITEST */ 


88 

89 

90 

Ii emai nad * 

92 * SETVME() * 

93 * * 

94 * ENVIRONMENT: GCC COMPILER V2.7.22 
: 

95 *LAST UPDATE: 24 FEBRUARY 1997 * 

96 *NAME: | THORSTEN LEONARDY * 

97 * PURPOSE: THIS FUNCTION OUTPUTS DATA TO THE 
STATUS REGISTER OF THE * 

98 * SPECIFIED VME BOARD. * 

FROM Sooo 2 eee */ 


100 VOID SETVME(UNSIGNED CHAR *BOARDADDRESS, UNSIGNED 
CHAR DATA) 


101 { 


102 BOARDADDRESS = BOARDADDRESS + 0X81; /* ACCESS 
STATUS REGISTER */ 

103 *BOARDADDRESS=DATA; /* WRITE DATA | 

104 RETURN; 

105} 


189 


106 


107 

108 

a ee ee ee ee * 

110 * TOGGLEVME() * 

111 * * 

112 * ENVIRONMENT: GCC COMPILER V2.7.2 
: 

113. * LAST UPDATE: 24 FEBRUARY 1997 * 

114 *NAME: |= THORSTEN LEONARDY * 

115 * PURPOSE: THIS FUNCTION PERFORMS AN XOR 
OPERATION ON THESTATUS * | 

116 * REGISTER OF THE SPECIFIED VME BOARD. 
r 

Alf 2 ena cone ee eee eee */ 


118 VOID TOGGLEVME(UNSIGNED CHAR *BOARD, UNSIGNED 
CHAR DATA) 


niet 
120 BOARD = BOARD + 0X81; /* ACCESS STATUS REGISTER */ 


12] *BOARD = *BOARD * DATA; /* TOGGLE BIT WITH BITWISE 
XOR */ 


he RETURN; 
Ze} 

124 

125 


126 


190 


ae en ee en oe etd x 


128 * INITBOARDS() * 
129 * : 
130 *ENVIRONMENT: GCC COMPILER V2.7.2 * 
131 *LAST UPDATE: 24 FEBRUARY 1997 * 
132 * NAME: THORSTEN LEONARDY * 


133. * PURPOSE: THIS FUNCTION INITIALIZES ALL VME BOARDS. 


TG 8 eee ae SSS */ 
135 VOID INITBOARDS(VOID) 
136 { 


1S UNSIGNED CHAR *P; 


138 INT Ix; 

139 

140 P=(UNSIGNED CHAR*)VIC_TTR; /* VIC TRANSFER 
TIMEOUT REGISTER = */ 

14] SP = OXF, /* DISBLE ALL WATCHDOGS | 

142 

143 P=(UNSIGNED CHAR*)VIC_ICR; /* VIC INTERFACE 
CONFIGURATION REG. */ 

144 *P=0X40; /* PREVENT DEADLOCKS, THIS IS A 
MUST! */ 

145 


146 P=(UNSIGNED CHAR*)VME9421+0X81;  /* ACCESS STATUS 
REGISTER FOR DI */ 


19] 


147 *P = 0X03; /* DISABLE SYSFAIL SIGNAL, SET 
GREEN */ 


148 


149 P=(UNSIGNED CHAR*)VME9210+0X81; /* ACCESS STATUS 
REGISTER FOR DA */ 


150 +P = AUS: /* DISABLE SYSFAIL SIGNAL, SET 
GREEN */ 

151 

152 

[a fe ee eee eee eee x 


154 * INITIALIZE ALL EIGHT QUADRATURE COUNTERS (WHEEL 
ENCODER) * 


155 eee een. eee * / 
156 


IS7 = P=(UNSIGNED CHAR*)VMECTRI; 


158 FOR (IX=0; IX<8; IX++) { /* READ MOTORS 
SUBSEQUENTIALLY */ 

159 —- *(P+3)=0X20: /* CR: MASTER RESET */ 

160 *(P+3)=0X48: /* IC: ENABLE COUNTING */ 

161 — *(P+3)=0XC1: /* QR: COUNT FULL CYCLE */ 

162  P=P+0X04; /* ACCESS NEXT COUNTER */ 

163 IF (IX==3) P=(UNSIGNED CHAR*)VMECTR2; /* ACCESS THE 
SECOND VME COUNTER */ 

164} 

165 


166 SIOOUT(O,"BOARDS INITIALIZED ..\N\R"); 
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167 

168 RETURN; 

169 } 

170 

171 /* MODIFIED ED MAYS 18 APR 97 */ 
172 UNSIGNED CHAR B2A(INT VALUE) 
foe 

174 UNSIGNED CHAR CHAR; 

175 

176 IF (VALUE <10){ 

177 CHAR = 48 + VALUE; 

178} 

179 ELSE { 

180 CHAR = 55 + VALUE; 

181} 

182 RETURN CHAR; 

isa} 

184 

185 VOID B2A2(UNSIGNED CHAR *S, UNSIGNED CHAR CC) 
186 { 

187 INT LOW, HIGH: 

188 

189 LOW =CC & OXOF: 


190 HIGH =CC/16; 


193 


19] 

192. *S =B2A(LOW); 

193. *(S-1) = B2A(HIGH); 
194 } 

eo) 

196 


OF 


[FR PR AR AR Ae Ae Ae ee AG A 2 eee a AR OR A ee A OR Ro A Oe eA RAR OR OR Oe GO 8 2 OR AGO OR OR AR OR OB A OE OB OK OE 
7K OOK OK OK OK OK OK OK OK OK OK 


198 ASSEMBLER ROUTINES 


L.-T a 
oe KKK AK / 

200 

201 ASM" 

202 EVEN 

203 EAT 

204 -GLOBL _PIHANDLER 

205 


206 _PIHANDLER: 


207 MOVE.L #OXFFF4800C,A1 /* LOAD APP-ICR INTO Al 
i) 

208 AND.B #0XFD,(A1) /* REMOVE PENDING IP-46 
INTERRUPT SIGNAL */ 

209 MOVE.L #0X1237,_PIFLAG /* SET PIFLAG VARIABLE 
oy 

210 RTE 


194 


22 


213 

DANA ho REO 2 ns ee * 
215 *CONVERTTOASCII() * 
216 * * 
217 * ENVIRONMENT: GCC COMPILER V2.7.2 * 
218 *LAST UPDATE: 02 MAY 1997 * 
219 *NAME: =THORSTEN LEONARDY * 


220 *PURPOSE: THIS FUNCTION CONVERTS AN UNSIGNED 


INTEGER TOITS ASCIT * 


221 * EQUIVALENT AND WRITES THIS INTO A STRING. 
* 

222  * NDIGITS NUMBER OF DIGITS TO CONVERT 
*K 

223 *DATA THE INTEGER TO CONVERT x 

224 *STR POINTER TO STRING 

DD Se nS rr */ 


226 VOID CONVERTTOASCH(UNSIGNED INT NDIGITS, UNSIGNED 
INT DATA, CHAR *STR) 


Zoe | 

228 UNSIGNED INT J; 

229 

230 STR=STR+NDIGITS-1; 


231 FOR (J=0;I<NDIGITS;I++) { 


1) 


232 «= *STR--='0'+ DATA %10; 

233. DATA=DATA/10; 

234} 

235 RETURN: 

236 } 

237 

238 

ROA lcte e  r e 3 
240 * READJOYSTICK() A 
241 * : 


242 j ENVIRONMENT: GCC COMPILER V2 


243. * LAST UPDATE: 02 MAY 1997 
244. * NAME: THORSTEN LEONARDY 2 


245 * PURPOSE: THIS FUNCTION READS THE THREE PORTS (A,B 
AND C) FROM THE * 


246 * INTEL 85C55 PARALLEL PORT 1 AND CONVERTS 
THEM INTO AN ASCII * 

247 * STRING. * 

DAME nance ann een */ 

249 


250 VOID READJOYSTICK(VOID) 
Zee 
252, UNSIGNED INT I,INDEX; 


253. UNSIGNED CHAR *CTRLPORT=(UNSIGNED CHAR*)PIOI_CTRL; 


196 


254 


UNSIGNED CHAR *DATAPORT=(UNSIGNED 


CHAR*)PIO!_ DATA; 


255 


256 


Zo], 


258 


UNSIGNED INT PIOPORT1[3]; 


DOUBLE._A=0.1, XA; YY, ZZ; 


*CTRLPORT=0X9B; /* SET ALL PORTS (A,B,C) INTO INPUT 


MODE (READ ONLY) */ 


ihe, 


JOYSTICK 


260 


261 


262 


263 


264 


265 


266 


Zoe 


268 


269 


270 


271 


Ze 


ZO 


274 


2/5 


INDEX=10; /* POSITION FOR X-DIGITS IN STRING 
oi 


FOR (1=0;1<3;I++) 


PIOPORT 1 [I] = *(DATAPORT+D); 


XX = (DOUBLE)PIOPORT1[0]-128.0; 
YY = (DOUBLE)PIOPORT1[1]-128.0; 
IF (XX >= 0.0) 
XX = XX*XX/100: 
ELSE 
XX = -XX*XX/100; 
IF (YY >= 0.0) 
YY = YY*YY/100: 
ELSE 
YY =-YY*YY/100; 
JOYSTICK.X = A*(XX) + (1.0-A)*JOYSTICK.X: 


JOYSTICK.Y = A*(YY) + (1.0-A)*JOYSTICK.Y; 


197 


276 
277 ~=IF (PIOPORT1[2]==0X03) 


278 SETVME((UNSIGNED CHAR *)VME9210,0X00); /* NO BUTTON 
PRESSED) 


279 ELSE{ 


280 SETVME((UNSIGNED CHAR *)VME9210,0X02); /* IF ANY 
BUTTON PRESSED */ 


281 } 
DSDy 

283 

284 

285 DOUBLE INSENSITIVE(DOUBLE Z) 
286 { 

287 IF(Z>= 10.0) 

288 | RETURN (Z- 10.0); 

289 ELSE 

290 IF (Z<=-10.0) 

291 RETURN (Z+ 10.0): 

292 +-ELSE 

293. RETURN 0.0: 

294 } 

295 

296 VOID DISPLAYJOYSTICK() 

297 { 


298 CONVERTINT(BCDSTRING+9, (INT)JOYSTICK.X); 


198 


Zo 
300 
301 
302 
303 
304 
305 
306 
307 
308 
309 


310 


BCDSTRING[3]='0'; 
BCDSTRING[4]='3;; 
BCDSTRING[6]='4;; 
BCDSTRING[7]='0; 


SIOOUT(0,BCDSTRING); 


CONVERTINT(BCDSTRING+9, (INT)JOYSTICK.OMEGA); 
BCDSTRING[3]='0;; 
BCDSTRING[4]='4; 
BCDSTRING[6]='4; 
BCDSTRING[7]='0; 


SIOOUT(O,BCDSTRING); /* OUTPUT UPDATED POSITION 


STRING TO SCREEN */ 


311 


BIZ} 


313 


314 


RETURN; 


RFRA III IO RO ICI IC I a AR ER RK 


2K AR OK OK OK OK OK OK OK Ok 


315 


316 


END OF UTILS.C 


2K 2K 2 2K 2 Ko OK 2K ok 2K 2 2K KE 2K 2K KK 2 2K 2 2K OK 2K OR 2 KK KK ORK OK OK KOR OK KK KOK KK OK KOK oe oR ok 2 Ko ok KK Kk OK OK OK 


St ee / 


199 





200 


APPENDIX I: SOURCE CODE (SERIAL.C) 


CO en ke * 

>  * 

oe * FILE: SERIA xe ‘i 

oe x 

5 * ENVIRONMENT: GCC COMPILER V2.7.2 zi 

cee LAST UPDATE: 26 FEBRUARY 1997 i 

fon) * NAME: THORSTEN LEONARDY : 

See PURPOSE: PROVIDES ROUTINES TOR SERIAL INPUT 
AND OUTPUT TO THE 68C681 + 

-_ * ON THE TAURUS BOARD. - 

10 K K 

11 *COMPILED: >GCC -C -M68040 -O SERIAL.O SERIAL.C* 

12 * 

15 ¥ ~~~ ----------+---------------- = 2 on nnn nnn nnn nse anna nn nna sa) 

14 

15 #INCLUDE "SHEPHERD.H" 

16 #INCLUDE "SERIAL.H" 

ny 

18 /* -------------------------------- ** 

GLOBAL VARIABLES 33 

20 ~=s*® -------------------------+------ a 

2) 


201 


22 /* UNSIGNED INT COUNTER; /* COUNT THE INTERRUPTS + 


23 UNSIGNED CHAR INPORTA; /* CHARACTER READ FROM SERIAL 
PORG 7 


24 

25 /* VT100 CONTROL SEQUENCES */ 

26 

27 /* POSITION CURSOR, CUP = ESC ['0''0' ; '0' '0' H */ 


28 UNSIGNED CHAR VTI00XY([9]={27,91,48,48,59,48,48,72,0}; /* POSITION 
CURS@R 77 


29 

30 /* ERASE IN DISPLAY ED TO CLEAR THE SCREEN */ 

31 UNSIGNED CHAR CLRSCR{5]= {27,91,50,74,0};  /* ESC ['2'J */ 
32 

33. /* ESC-SEQUENCE EL (ERASE IN LINE) TO ERASE A LINE */ 

34. UNSIGNED CHAR CLRLINE[6]= {5,27,91,50,75,0}; /* ESC ['2'K */ 
35 

36 /* ESC-SEQUENCE PRINT SCREEN (ESC [ I) */ 

37 UNSIGNED CHAR PRTSCR[4]= {27,91,105,0}; /* ESC [1*/ 

38 

39 

40 /* ESC-SEQUENCE SGR (SELECT GRAFIK RENDITION) (ESC [ 0 M) */ 


41 UNSIGNED CHAR CURSOROFF{5]= {27,91,0,109,0}; | /* CURSOR BLINK 
OFF */ 


42 


3g Aaa oa tec caeeed ev rerges atts pate eee ore * 


Ze 


44 


45 


46 


47 


48 


49 


50 


Dl 


2 


53 


54 


Ss 


56 


Sy 


58 


a 


60 


61 


62 


63 


64 


65 


66 


* SIOOUT( * 


* ENVIRONMENT: GCC COMPILER V2.7.2 ss 
MENS POATE: 07 ANU ARY 1997 * 
* NAME: THORSTEN LEONARDY * 


PURPOSES THis FUNCTION OUTPUTS A SERING 


TO ONE OF THE TWO SERIAL e 
‘ ROK ES. ‘s 
K ok 


* HOSTFLAG 0-> OUTPUTS TO CONSOLE (PORT A) * 


‘J 1-> OUTPUTS TO HOStaateORT &) # 

*K ok 
7 POINTER TO THE OUTPUT STRING 

*K *K 
1 eee .. eee * / 


VOID SIOOUTUNT HOSTFLAG, UNSIGNED CHAR *S3) 


{ 
UNSIGNED CHAR *P=(UNSIGNED CHAR *)CONSOLE; 


IF (HOSTFLAG) P+=8;  /* ACCESS HOST REGISTERS */ 
/* OTHERWISE ACCESS CONSOLE */ 
WHILE(*S) { 
WHILE ((*(P+1)&4)==0); /* SRA: WAIT UNTIL TX READY */ 


pba) ey J OU TREE Cr Gite a7, 


203 


67 


68 


69 


70 


7 


ie. 


73 


74 


1D 


76 


a 


78 


Te 
SCREEN. 


80 


81 


82 


83 


84 


85 


86 


87 


88 


89 


RETURN; 


} /* END OF SIOOUT */ 


fe Sere hn ns kee ele * 

* GOTOXY( * 
* ENVIRONMENT: GCC COMPILER V2.7.2 * 

* LAST UPDATE: 14 FEBRUARY 1997 * 
*NAME: | THORSTEN LEONARDY * 

* PURPOSE: THIS FUNCTION POSITIONS THE CURSOR ON THE 
* * 
*X ROW FOR CURSOR POSITION (X=0..20) * 
*Y COLUMN FOR CURSOR POSITION (Y=1..80) * 

¥ * 
in Te ec RT ee, Be Se a, oe + 


VOID GOTOXY(INT X, INT Y) 


IF ((X>0)&(X<81)&(Y>0)&(¥ <33)) { 


204 


90 


2 


2 


Do 


94 


as, 


96 


97 


98 


oO 


100 


101 


102 


103 


104 


105 


106 


107 


108 


109 


110 


VT100XY[2]=48+X/10; 
VT100XY[3]=48+X% 10; 
VT100XY[5]=48+Y /10; 
VT100XY[6]=48+Y%10; 


SIQOOUTO,VII00XY); 7 a@WIPUT PSCAPE SBOQUENGE *7 


RETURN; 


* SIOINITO * 
* ENVIRONMENT: GCC COMPILER V2.7.2 * 
* LAST UPDATE: 26 FEBRUARY 1997 * 
*NAME: | THORSTEN LEONARDY * 


PURPOSE: Trils FUNCTION INITIAVIZES BOTH 


SERIAL PORTS. IN ADDITION, = 
- PORT A(CONSLE}IS INEABIZED LOR 
INTERRUPT DRIVEN I/O i 
Teen O Ne orto: 5 cece * / 


VOID SIOINIT(VOID) 


205 


111 


UNSIGNED CHAR *P=(UNSIGNED CHAR*)CONSOLE;/* BASE ADDRESS 


FOR 68C681 DUART */ 


le 


Pe 


114 


i. 


116 


ee 


118 


LONG *VADR; /* FOR VBA REGISTER ENTRY sa 
Fe ae ance ccs ee * / 

/* INITIALIZE CONSOLE (PORT A) * / 

YE ee ee Pe) ee ae * / 


/* ATTENTION: THESE SETTINGS HAVE TO AGREE WITH TH 


SETTINGS FOR a 


*/ 


*/ 


BIT 


“y 


Ho 


120 
av 


PA 


WZ 


125 


124 

* 
12> 
126 


ee 
128 
2) 


130 


/* YOUR TERMINAL (1.E. LAPTOP COMPUTER) */ 


*(P+2)=(UNSIGNED CHAR)0X2A; /* CRA: RESET RX,DISABLE RX & 


*(P+2)=(UNSIGNED CHAR)OX1A; /* CRA: RESET MR POINTERS 
*(P+0)=(UNSIGNED CHAR)O0X13; 7/* MRIA: RX CONTROLS Rita 
ie 8 BITS, NO PARITY a 


*(P+0)=(ONSIGNED CHAR)0X07; /* MR2A: NORMAL MODE, 1 STOP 


*(P+1)=(UNSIGNED CHAR)OXBB; /* SET BAUD RATE 9600 BAUD 
*(P+2)=(UNSIGNED CHAR)OX15; /* ENABLE RX AND TX 
je hp LA cea a SEO PRR EEC Ee ORL Pe 855, Stare a sg RO - 
/* INITIALIZE HOST (PORT B) o/ 


206 


ol eats 5 Se ene ee ee ee eos See eee * / 


2 *(P+10)=(UNSIGNED CHAR)OX14A; /* CRB: RESET MR POINTER 


ef 

133 *(P+8)=(UNSIGNED CHAR)0X13; /* MR1B: NO PARITY, 8 BITS 
o/ 

134. *(P+8)=(UNSIGNED CHAR)0X07;_ —- /* MR2B: NORMAL MODE, 1 STOP 
BIT */ 

135. *(P+9)=(UNSIGNED CHAR)OXBB; /* SET BAUD RATE 9600 BAUD 
/ 

136 *(P+10)=(UNSIGNED CHAR)0X15; /* CRB: ENABLE RX AND TX 
ef 

137 

138 

ee * / 

140 /* YY FOLLOWS THE INTERRUPT SPECIFIC PART FOR PORT A 
/ 

A 2c i eet */ 


142  *(P+5)=(UNSIGNED CHAR)0X02;. _/* ISR: SET INTERRUPT MASK FOR 
RXRDY A */ 


143 *(P+12)=0X60; /* TVR: PLACE INTERRUPT VECTOR * / 

144 /* 0X60 ACCESSES VBA AT BASE+0X180 */ 

145  VADR=(LONG*)0XFFE40180; /* VBA ADDRESS FOR INTHANDLER 
7 

146 *VADR=(LONG)INPORTAHANDLER;  /* WRITE ADDRESS INTO VBR 
= / 

147 

148 P=(UNSIGNED CHAR*)ISM_SERIAL; /* ISM CONFIGURATION 


REGISTER FOR SIO */ 


149 *P=0X09; /* INTERRUPTS TO 68040 ON LIRQ-1 7) 


207 


150 


on 
ISM */ 


152 
ho 
154 
[55 
156 
Dor 
158 
ae 
160 
161 
162 
163 
164 
165 
166 
167 
168 
169 
170 


ed 


P=(UNSIGNED CHAR®*)VIC_LIRQ1; = /* VIC068 LICR FOR LIRQ-1 FROM 


*P=0X01; /* ASSERT IRQ-1 FROM VIC TO 68040 */ 


[RRIF I IIR A AAA RA ATA AAA AAT AAA AAA TATA AA TA AA AAA AAA AA AA ARAFAT TTR HR 


Hoo OEE RR@U FINES 


RAEI AAA HEA AAA A AAA AAA A AAA AT AAA HAHA AAA AAA AAAI RAAT AA IAA HAHA AAA HAH HER A 


* INPORTAHANDLER() * 

* . 

* ENVIRONMENT: GCC COMPILER V2.7.2 * 
* LAST UPDATE: 27 JANUARY 1997 * 
*NAME: = THORSTEN LEONARDY + 


PURPOSE: INTERRUPT HANDLING ROUTINE FOR INTERRUPTS 


FROM 68C681 DUART * 


2 * IT READS A CHARACTER INPUT FROM THE KEYBOARD INTO 
VARIABLE * 


BT Dia INPORTA, INCREMENTS A COUNTER, AND OUTPUTS THE 
SEIARACTER * 


174 * TO THE SCREEN. IF A CR IS TYPED AT THE KEYBOARD, AN 

175 * ADDITIONAL LINEFEED (0X0A) IS ADDED TO THE <CR> 
(OX0D). * 

176 * * 

I a * / 

178 

179 ASM(" 

180 

181 EVEN 

182 TEXT 

183 .GLOBL _INPORTAHANDLER 

184 


185 _INPORTAHANDLER: 


186 

187 LINK  A6,#-128 /* ALLOCATE 184 BYTES ON STACK TO... */ 

188 FSAVE AG6@(-128) 

189 MOVEML  D0-D7/A0-A5,SP@- /* SAVE REGISTERS (14*4 BYTE) 
*/ 

190 

191 MOVE.L #0XFFF4A000,A2. ~— /* BASE ADDRESS OF 68C681 DUART 

/ 
192 MOVE.B 3(A2),D2 /* RHR_A: READ CHARACTER ... */ 


209 


193 MOVE.B D2, INPORTA _—/*.... AND COPY TO INPORTA */ 
194 

195 MOVEML SP@+,D0-D7/A0-A5 
196 FRESTORE A6@(-128) 

197 UNLK AG 

198 

199 

200 RTE 

201 "); 

202 

203 

204 


205 Gf AI SA ei ce heck tack A A ea eke oc hia ceeokete ote cheke eects cote ka A Aca ha 


206 END OF SERIAL.C 


207 Be eae tee reacties eerie hve cet nee ok tee ete toate calc caemame eects kek cheikh ala 


210 


APPENDIX J: SOURCE CODE (CONSOLIDATED HEADER FILES) 
The following code was modified by: Professor Kanayama, Thorsten Leonardy, 


Edward Mays, and Ferdinand A. Reid. 
| /* shepherd.h */ 

Z 

3 #ifndef SHEPHERD_H 


4 #define SHEPHERD_H 


10 #define VME9210 Oxffff0400 /* Base Address analog outto servo */ 
11 #define VME9421 Oxffff0000 /* Base address datainfromservo  */ 
12 #define VME2170 Oxffffff00 /* Base address data out to servo | 

13 #define VMECTRI Oxffff6000 /* VME Counter for driving motor my 


14 #define VMECTR2 Oxffff6100 /* VME Counter for steering motor  */ 


18 * defines for general Interrupt Handling * 


2 


20 

21 #define VIC_LIRQI Oxfff44027 /* VIC068 Register for LIRQ-1 */ 
22 #define VIC_LIRQ2 Oxfff4402b /* VIC068 Register for LIRQ-2 */ 
23 #define VIC_LIRQ3 Oxfff4402f /* VICO68 Register for LIRQ-3 */ 
24 #define VIC_LIRQ4 Oxfff44023 /* VICO68 Register for LIRQ-4 */ 
25 #define VIC_LIRQ5 Oxfff44037 /* VICO68 Register for LIRQ-5 */ 
26 #define VIC_LIRQ6 Oxfff4403b /* VICO068 Register for LIRQ-6 */ 
27 #define VIC_LIRQ7 Oxfff4403f /* VICO68 Register for LIRQ-7 */ 
28 

29 #define VIC_TTR Oxfff44043 /* Transfer Timeout Register */ 

30 /* see p. 4-2 TAURUS Manual */ 

31 #define VIC_ICR Oxfff440af /* VIC Interface Configuration */ 
a2 

33 


34 #define enable() asm("move.w #0x2000,sr") /* enable interrupts */ 

35 #define disable() asm("move.w #0x2700,sr") /* disable interrupts */ 

36 

oF 

38 /* defines for Vector base register entries */ 

39 #define VBA_TIMER Oxffe40130 /* Vector table address for Timer-5 ISR */ 


40 #define VBA_PI  Oxffe40118 /* Vector table entry for IP interrupt */ 


Dae 


46 

47 #define ISM_TIMER Oxfff48004 /* ISM Configuration Register for Timer A */ 
48 #define ISM_PI Oxfff48008 /* ISM Configuration Register for PI  */ 

49 #define ISM_SERIAL Oxfff48001 /* ISM Configuration Register for serial IO */ 
50 

51 #define APP_ICR Oxfff4800c /* abort/processor interrupt control register */ 

52 

5. 


54 


5] * ------------------------------------------------- =) 

58 #define PIO] _ CTRL Oxfff40003 /* control register for PIO-1 */ 

59 #define PIO! DATA Oxfff40000 /* data register for PIO-1 Port A */ 
60 #define PlIO2_CTRL Oxfff40007 /* control register for PIO-2 */ 

61 #define PIO2_DATA Oxfff40004 /* data register for PIO-2 Port A */ 
62 


2M 


65 * Base Addressees for 68030 Input/Output Program * 


66 * as outlined in Taurus Manual, Chapter 6 : 


68 #define IOP_CMDBLK  Oxffe00000 /* address for IOP Command Block */ 


69 #define IOP_START  Ox0Ol1 /* command to start IOP */ 

70 #define IOP_STOP 0x00 /* command to stop IOP */ 

71 #define IOP_COMPLETE 0x80 /* mask for operation complete */ 
a2 

73 #define IOPB_CONFIGURE Oxe0 /* command to configure IOBP */ 
74 #define IOPB_UNIT_OMNIO 0x10 /* unit # for omnimodule #0 */ 
15 [* ------------------------ 2-2 nn nnn nnn nn nnn nnn nnn nnn nn nn nnnne - 


78 

79 

80 /* Input/Output Parameter Block structure, according Taurus Manual, p. 6-4 */ 
81 typedef struct { 

82 unsigned char cmd; /* command */ 

83 unsigned char error; /* error status */ 


84 unsigned short options; /* options */ 


214 


85 unsigned short reserved; /* reserved, do not use */ 

86 unsigned char unit;  /* unit number */ 

87 unsigned char destUnit; /* destination unit */ 

88 unsigned long blockNumber;/* logical Block number */ 

89 unsigned long txCount; /* Transfer count, # of bytes to transfer */ 
90 unsigned long *ptrSrc; /* address of source */ 

91 unsigned long *ptrDst; /* Address of destination */ 

92 }IOPB; 

93 

94 /* Command Block structure according to Taurus Manual, p. 6-3 */ 
95 typedef struct { 

96 unsigned char cmd; /* status and command register */ 

97 unsigned char reserved[3]; /* not yet used */ 

98 IOPB *ptrToIOPB; /* pointer to IOBP */ 

99 }CMD_BLOCK; 

100 

10] 

102/* Omnimodule support block structure according to Taurus Manual p. 6-12 */ 
lO3typedef struct { 

104 unsigned long options; /* 4 bytes options, unused wh 
105 unsigned long *ptrInit; /* pointer to initialization routine = */ 


106 unsigned long *ptrTask; /* pointer to task i 
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107 unsigned long *ptrIntr; /* pointer to interrupt servicing routine */ 

108 }OSB; 

109 

110IOPB iopbOMNIO; /* IOBP for Omnimodule 0 (used for serial I/O to VT100 */ 


1110SB osbOMNIO; /* OSB for Omnimodule 0 (used for serial I/O to VT100 */ 


iy 

113/* --------------------------------------------------------------------------- uy) 

114/* --------------------------------------------------------------------------- 7] 

115 

116unsigned int intCounter, testCounter; /* count the interrupts i 
117unsigned int demo; /* switch to run demo see driver() in movement.c */ 


118unsigned short timer_in_ms; /* desired timer period in ms */ 


119 

120 

UU a ae *k 

122 * definitions for inertial measurement routines (imu.c) : 
ieee ee eae eee ** / 

124 


125/* added 10 Sep 97 */ 
126typedef struct { 
127 unsigned short ax; /* linear acceleration in x-direction */ 


128 unsigned short ay; /* linear acceleration in y-direction */ 
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129 unsigned short az; /* linear acceleration in z-direction */ 

130 unsigned short omega_z; /* angular velocity in z-direction */ 

131 }IMU; 

132 

133IMU imu; /* stores most recent IMU data (updated with */ 
134 /* every 1Oms timer interrupt =) 


is 


137 * definitions for Joystick Control, (Leo, 05/10/97) : 


Loe 

140typedef struct { 

141 double x; /* x position (or velocity) */ 
142 double y; /* y position (or velocity) */ 
143 double omega; /* angular velocity */ 
144 unsigned char state; /* status of parallel port 1, channel C */ 
145 }JPOINT; 

146 JPOINT joyStick; /*global*/ 

147 

148typedef struct { 

149 double x; 


150. double y; 


ZN), 


151 } point; 

es 

3 

154typedef struct { 
155 point coord; 
156 double heading; 
157 double kappa; 
158 } Configuration; 
159 Configuration vehicle; /*global*/ 
160 | 
16ltypedef struct { 
162 double Speed; 
163 double Theta; 
164 double Omega; 
165 }vehicleMotion; 
166 vehicleMotion motion,motionO; /*global*/ 
167 

168typedef struct { 
169 double rho; 

170 double alpha; 
171} polar; 


lez 
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Lee] 

178/* write these masks to VME2710 at address Oxffffff00 in order to make */ 
179/* the specific motor drive! May wish to logical OR with previous settings */ 
180 

18l#define TURN_FR 0Ox00004000 = /* turn wheel 1 (front right) */ 
182#define TURN_FL 0x00020000 /* turn wheel 2 (front left) */ 
183#define TURN_RR 0x00100000 = =/* turn wheel 3 (rear right) */ 
184#define TURN_RL 0x00800000 = /* turn wheel 4 (rear left) */ 

185 

186#define DRIVE_FR 0x00000004 /* drive wheel 1 (front right) */ 
18/#define DRIVE_FL 0x00000020 /* drive wheel 2 (front left) */ 
188#define DRIVE_RR 0x00000100 = /* drive wheel 3 (rear right) */ 
189#define DRIVE_RL Ox00000800 /* drive wheel 4 (rear left) */ 

190 

19 1#define ALL_WHEELS 0x00924924 = /* select all wheels for turning */ 

is /* and driving */ 


iS 


Za 


195 * function definitions * 


197 

198void setVME(unsigned char *board, unsigned char data); 
199void toggle VME(unsigned char *board, unsigned char data); 
200void initBoards(void); 

201 void piTest(void); 

202void piHandler(void); 

203 void advanceCount(); 

204 /* global variable to make joystock coordinates accessible */ 
205 

206#define ARRAY_SIZE 4 

207#define DegreesToRads 0.0174532925 

208#define RadsToDegrees 57.29577951308232 

209#define DeltaT 0.01 

210 

211 

212double desiredAngleRates[ARRAY_SIZE], 

213 desiredAngleRatesO[ARRAY_SIZE]}, 

214 desiredSpeeds_F[ARRAY_SIZE], 

as desiredAngleRates_F[ ARRAY_SIZE]}, 


216 desiredSpeeds[ARRA Y_SIZE], 


220 


27 actualSpeeds[ARRAY_SIZE], /* 28 May ejm */ 
218 actualAngleRates[ARRAY_SIZE]}, 
219 DigitToCmDrive[ ARRAY_SIZE], 
220 Display_Speeds[ARRAY_SIZE], 
221 Display_Steers[ ARRAY_SIZE]}, 
222 

223 desiredAngles[ARRAY_SIZE]}, 
224 desiredAnglesO[ARRAY_SIZE], 
225 actualAngles[ARRAY_SIZE]; 
226 

22/7short Steer_Digits[ ARRA Y_SIZE]}, 


228 Speed_Digits[ARRA Y_SIZE]; 


229 

230double WheelDriveAct[ ARRA Y_ SIZE}, 
231 WheelDriveDes[ ARRA Y_SIZE]}]; 
Ze. 


233unsigned long int WheelDriveActO[ ARRA Y_SIZE]}, 


234 WheelDriveAct|{[ARRAY_SIZE], 
235 driveReadings[ARRAY_ SIZE]; 
236 

237double WheelDirAct{ ARRA Y_ SIZE}, 
238 WheelDirDes[ARRA Y_SIZE], 


Zo 


oo PreviousCountSpeed[ARRA Y_SIZE], 
240 PreviousCountSteer[ARRA Y_SIZE]; 
241 


242unsigned long int WheelDirActO[ARRAY_SIZE], 


243 WheelDirActI[ARRAY_SIZE], 
244 steerReadings[ARRAY_SIZE]; 
245 

246int mode, 


247 oldMode, 

248 modeOstate, 

249 modeSstate, 

250 modeTstate, 

251 Flag, 

252 oldFlag, 

253 edCounter, 

254 _hallSensor3; 
255unsigned int intCounter, testCounter; /* count the interrupts 
256 

257 

258 

259/*unsigned long int */ 


260double previousCount,previousCountSteer, Omega_Speed, 


ee 


a) 


261 previousCountSpeed;/*previousCount represents infinity */ 


262 


263double KI[ARRAY_SIZE], 


264 K2[ARRAY_SIZE], 

265 K3[ARRAY_SIZE], 

266 K4[ARRAY_ SIZE], 

267 K6[ARRAY_SIZE]; /* slope based on input units vs output velocity, */ 
268 /* input range from O- 1020, feedback constant */ 

269 /* K3 is the inverse of (86.975velocity/1020 digit) */ 

270 

Zl 

2/2#endif 

273 


275 End of shepherd.h 


Zi 


278 


279 #ifndef_ MOVEMENT_H__ 


280#define _MOVEMENT_H__ 


28 | 


282#include "shepherd.h" 


225 


283 


284 

285#define PI 3.14159265358979323846 

286#define DPI 6.28318530717958647692 /* PI*2 =) 
287#define HPI 1.570796327 /* Pl/2 7 
288#define QPI 0.785398163 eA 


289#define QPIby500 0.0015707963 
290 /* QPI/(5 seconds/deltaT) */ 
2911 

292 

293 

294 

295 double wheel_speed[4], wheel_angle[4]; 
296 

297 void initMovement(); 

298 void setupPolar(polar []); 

299void wheelMotion(); 

300 void bodyMotion(); 

301 void driver(); 

302 void joystickMotionInterface(void); 

303 


304 
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305extern double desiredAngleRates|], 


306 desiredS peeds[], 

307 PreviousCountSpeed[], 
308 PreviousCountSpeed[], 
309 PreviousCountSteer[]; 
310 

311 polar whp[4}]; 


312double pathLength,thetaDot,omegaDot,speedDot; 
313 

314 

315 

3 16/* RHEE TIOMS for tangential Motion *******#*H KK X/ 
Be 

318 

319double sigma; 

320double radius; 

321 

322double ai[4], bil4]; 

323typedef struct { 

324 Configuration config; 

325 point center; 


326 double radius: 
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327 double ae 

328 double b; 

329 double ce 

330} LINE; 

331 

332static LINE currentPath; /* holds the current path element values */ 
333 Configuration incremental Motion, holdVehicle; 
334static double deltaS; 

335 

336void tangentialMotion(); 

337 void circularArc(double length, double alpha); 
338void defineConfig(double x,double y,double theta,double kappa); 
339void compose(); 

340double steer(); 

341 void constants(); 

342double Psi(point p1,point p2); 

343 double distance(point p1,point p2); 

344 void initTangent(); 

345 

346#endif 

347 


348 
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349 

350 

351#ifndef MOTOR_H__ 
352#define _ MOTOR_H__ 
353 

354#include "shepherd.h" 


a2) 


357 * Base Addressees for accessing Servo Control Cards * 


358 * Used in Home Testing 


360#define SteerDrivelInteract .02 /* used to give stability to wheel */ 
361 

362#define RadRateTodigit 195.3789 /* digit/radpersec*/ 

363 


364#define digitToRadDrive -6.015495746e-5 


365 /* driving constant rad/count = DPI/104450 May8 */ 
366 /* Experimental Results by Ed Mays May 7 */ 
367 /* Wheel | count = 104456 zi 

368 /* Wheel 2 count = 104435 a 

369 /* Wheel 3 count = 104454 7 

370 /* Wheel 4 count = 104455 +} 


oy 


Bol /* Average count = 104450 | 

2 /* cf. 2048 * 51 = 104448 = / 

373#define digitfToCmDrive 0.001 1369287 

374 /* driving constant cm/count = digitToRadDrive*18.9cm 5/8/97 */ 
57D 


376#define digitToRadSteer -6.8 1769239 le-5 


397 /* steering constant rad/count = DPI/(2048*45) 19 Apr */ 
378 

379#define SteerFBGain 0.000; /* steering feedback gain a) 
380#define DriveFBGain 0.000; /* driving feedback gain a) 


381#define DigitsHigh 1023 

382#define DigitsLow -1024 

383#define WheelRadius 18.9 /= prev del in em 7/ 

384#define VME9210 Oxffff0400 /* Base Address analog outtoservo */ 
385#define VME9421 Oxffff0000 /* Base address datainfrom servo */ 
386#define VME2170 Oxffffff00 /* Base address dataouttoservo */ 
387#define VMECTRI Oxffff6000 /* Counter ay 

388#define K5 87.4 /* control feedback constant (cm/sec) variable 28 May eym */ 
389#define DriveFeedBackGain 0.8 /*.8 control drive feedback gain 28 May eym */ 
390#define angularK3 0.96963 /* digit/rotational speed (rad/sec) */ 

391 #define steerFeedbackGain 100.0 /* steering Feedback gain */ 


392#define angularK5 5.29906 
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393#define angleFeedbackGain 1000.0 


394 
395extern unsigned char clrLine[6]; /* ESC-Sequence for clear line a 
396extern char bwheeldrivecdString[];/* defined in shepherd.c | 


397extern unsigned char bcdString[]; 

398 

399double Drive_Feedback[ARRAY_SIZE]}; 
400 


401extern double desiredAngleRates[], 


402 desiredSpeeds{], 

403 PreviousCountSpeed[], 
404 PreviousCountSteer [], 
405 DigitToCmDrive[], 
406 Display_Speeds[], 

407 Display_Steers[], 

408 desiredAngles[], 

409 actualAngles[]; 

410 

41 ]/* -------------------- ° 


414void driveSpeed(short []); 


a0 


415void driveSteer(short []); 
416void driveMotors(); 

417void wheelDrive(void); 
418void allStop(void); 

419void updateEncoders(void); 
420void update WheelDrive(void); 
421 void update WheelSteer(void); 
422void displayDirections(void); 
423 void displaySpeed(void); 
424int readSteerEncoders(unsigned long int []); 
425void testDrive(void); 

426void readEncoders(void); 


427 void accumulatedriveSpeed(); 


428 void displayDriveAngle(); /* added 15 may */ 
429void drivingFeedback(); /*28 Mayeym */ 
430double velocityReferenceTable(double,int); /*28 Mayejm */ 
431 void steeringFeedback(); /* 4June  */ 

432double rateReferenceT able(double); /* 4June = */ 

433 void computeActualRates(); > une Wi 

434int convertDifference(int); (>) Junetermnn 

435 


436void alignWheels( void); 
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437 void clearShaftEncoder(unsigned short motors); 
438 void read WheelStatus(unsigned char *array); 
439 

440#endif 

44] 

442 

443 

444/* Timer.h */ 

445 

446#ifndef TIMER_H 

447#define TIMER _H 

448 

449/* Defines for Timer control */ 


450#define TIMER_CTRL Oxfff41002 /* Control register for Timer A cy 


451#define TIMER_DATA Oxfff41000 /* Data register for Timer A ey, 

452 

453 

454/* settings master mode register according to fig. 1-12 =f 

455#define TIMER_MASTER_MODE Oxbaf0 /* timer master mode register 3 
456 /* b=BCD count, 16 Bit data bus pf 

457 /* 4=divide by 4 ss 

458 /* 8 = Source F4 (divide by 1000) Si) 


2 )4) 


459 


460 


461 


/* 0 = don't care ssf 


462/* settings for counter mode register according to fig. 1-17 */ 


463#define TIMER MODE Ox0f31 /* Counter Mode Register Bit Assignment */ 


464 


465 


466 


467 


468 


469 


/* 0 =no gating, count on rising edge */ 
/* 8 = Source F4 (divide f by 1000) */ 
/* 3 = BCD repetitive count,reload load */ 
/* 2 = count down, toggle TC oy, 


/* or 1 = count down, active high Terminal Count Pulse */ 


474 void TimerHandler(void); 


475 void timerStart(void); 


476 


477 


478 


479#endif 


480 


Zoe 


481 


[76 EAB AR A Ae A OK Oe ee OB ee oe ee OK Ok ik ee Ok ke A Oe AC OK ie ie eA Oo oe Ae ee AS A OO 2k OE Oo oe OK i OR KOK OK KOK 


482 End of timer.h 
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484 

485 

486 

487 

488#ifndef _ MATH_H__ 
489#define MATH H__ 
490 

491 

492 

493 double cos(double x); 
494double sin(double x); 
49S5double atan2(double x, double y); 
496double atan(double x); 
497double new_sqrt(double x); 
498double new_abs(double x); 
499double norm(double angle); 


SO0double min (double, double); 


Zoo 


501 double max (double, double); 
502#endif 


503 


S07 = hile: SERIAL .H - 

508 * : 

509 * Environment: GCC Compiler v2.7.2 _ 
510 * Last update: 13 March 1997 * 

511 * Name: Thorsten Leonardy z: 


512 * Purpose: — Header File for ‘serial.c’ = 


514 

515#ifndef _ SERIAL_H__ 

516#define _ SERIAL_H__ 

De) 

518 

519#define CONSOLE Oxfff4a000 /* Base address 68C681 DUART */ 


520 


522 * function definitions * 
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524 
525 void inPortAHandler(void); /* interrupt handler */ 
526 void sioInit(void); /* initialize DUART = */ 


527 void sioOut(int hostFlag, unsigned char *s); /* Output a string */ 
528 void gotoX Y(int x, int y); /* position cursor a) 

oo 

530#endif 

531 


a BG A BO OR A AS AS OS AR OR AR AS OK OB Os Oe OS OB Oe AOR OB os 2s oi os 6 os 2s A el oe ok AS AS os oR oR AR OR i oR oe os oh oe oO of oe of oh 2 OB oR oo OR OR oe 


533 End of serial.h 


S54 FREER EREARAREAKARARARAAAAAAAREKARARK A KAA KAKA KK HA FAK KE KR KK Bh eK EK KK 


a) 

536 

537/* -------------------------------------------- $22 2-2 - = 222-22 === === = s 
Soo ss 

Bo) File: elie Sear - 

540 * : 

341 * Environment: GCC Compiler v2.7.2 2 
542 * Last update: 13 March 1997 = 
543 * Name: Thorsten Leonardy ss 
544 * Purpose: Header File for ‘utils.c’ af 
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546 
547#ifndef _ UTILS_H__ 
548#define _ UTILS_H__ 


549 


2 

554void setVME(unsigned char *board, unsigned char data); 
SS55void toggle VME(unsigned char *board, unsigned char data); 
556void initBoards(void); 

557 void piTest(void); 

558 void piHandler(void); 

559/*void readClock(void); */ 

560/*void WRITE_CLOCK(void); */ 

561 

562/* Modified 18 Apr */ 

563unsigned char b2a(int); 

564 void b2a2(unsigned char *, unsigned char); 

565 void convertToASCII(unsigned int ndigits, unsigned int data, char *str); 


566void readJoyStick(void); 


236 


567double insensitive(double z); 
568 

569#endif 

570 


57 | [Rw A A A AE OK 2 2 Oe A 2K OK KR OK 2 AK OR A OK OK 2 A OK OR OK EK OK 2 OR OK RK OK RK KKK KK 


572 End of utils.h 


573 ate are eRe oh Rio 2 oF °E 2 OE °F OR Ae oR A OR A OE A 2 2h OB 2h 2 OE OR Ae OR he oR Ae OE he ob OR OP Ae OPH OR EO A OR RE TR Ee OR OE Ah hohe ho oh HH | 


23) 
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APPENDIX K: SHEPHERD OPERATING MANUAL 


OVERVIEW 


The Purpose of this document is to provide a quick guide for doing downloads for testing 
or Other purposes. For a more detailed guide see the Shepherd Operators Guide (SOG). 


The Shepherd compilation and download process is a four step process: 
- Compile executable on workstation. 
- FTP S-Records to laptop. 
- Use Windows 95 HyperTerminal program for direct connection. 


- Run the program once download complete. 


Compile Executable on Workstation 


1. Once you have logged in on the Shepherd account, then use the xinit command to 
generate the X-Windows environment. 


2. In the large terminal window type "cap" at the UNIX prompt and press the return key. 


3. The alias "cap" logs you onto capella (the standard login script will scroll by). The 
Shepherd group uses capella (server) because of the nature of the cross compilation used 
for the "Taurus board" and Motorola 68040 CPU. 


4. Next, in the large terminal window type "taurus" at the prompt and press the return 
key. 


5. The alias "taurus" sets up the environment for compilation and print services. 


6. Next, in the large terminal window type "cd srk" at the prompt and press the return 
key; this takes you to the Shepherd Real-time Kernel (~shepherd/srk) directory. While in 
srk you can modify or edit the require files with your favorite editor (e.g., xemacs or 
nedit). Once you have completed your work, save your files and compile. See figure] on 
the next page. 
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Figure 1: The Unix Workstation Environment 


7. Compilation is done through the use of a makefile. Hence, to compile all you must do 
is type "make comp" at the prompt and press the return key (this will either succeed or 
fail). If the compilation fails work the errors provided by the compiler and compile again 
(an iterative process). Once, the compilation 1s a success you are ready to FTP the S- 
records to the laptop. 


FTP S-Records to Laptop 


8. To begin to FTP the S-records to the laptop a few items must be accomplished. First, 
the robot power must be switched on (levers a, b, and c on the power supply in the "up" 
or closed position on the physical robot; provides power to the robot and charges the 
batteries). 


240 


See figure 2 (note the "up" position below represents the down position on the physical 
robot). 


Shepherd Rotary Vehicle 
Power Supply 


Shepherd 
Rotary Robot 








| eae sat 
peers 


Figure 2: Power Supply Diagram 


9. Secondly, the laptop must be on and connected to the local network via the PCMIA 
card (ethernet). Press the laptop "On" button. 

10. Ensure the laptop powersupply is plugged in, and connected to the laptop. 

11. Ensure the therenet cable is properly connected to the to the PC card. 


12. After booting our laptop will prompt you to login as guest-- just “click on the 
cancel" button. You should see the Windows desk top on the laptop (figure 3 below). 
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Figure 3: Windows Desk Top 


13. The screen print below describes the way the windows should look. Now double 
click onthe WS_FTP95 shortcut to open the ftp tool. 
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Figure 4: Windows WS_FTP95 Shortcut 


14. At this point the ftp tool opens. Click on the "OK" button. All information has been 
previously set for you(e.g., hostname,UserID, and Password). See figure 5. 


15. The next Window has a split panel showing the Remote System (workstation) and 
the Local System (the laptop). The directories have been saved, so they always open to 
the correct directories. The file to be ftp'd is shepherd.TXT, it will be ftp'd from the 
remote system (~shepherd/srk) to the local system (c:\shepherdump). The file 
shepherd.TXT contains the S-Records that will be eventually downloaded to the actual 
robot CPU. To accomplish the ftp highlight the file to be transferred with your mouse 
and click on the arrow that points left (See figure 6). The file transfer usually takes 
about .3 seconds. 
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C:\Program Files\¥S FTI v | 


Session Profile 


Profile Name: fire r| [ oe | Chabir 
| iene __Dekte | seve | New | Cancel i 
| | ifs] comp. 
7 Host Name: 131. 120.1.13 Advanced... 
. Host Type: | Automatic detect ~| Help 
User ID: [shepherd IT Anonymous Login 
Passwort: Jo IV Save Password : 
; Flenane 
Account| Iv Auto Save Config : 
| IEE] US_F” Initial Directories a 
| VS Fa Remote Host:{/tmp_mnt/users/tes/shepherd/ Caltech 


Local PC:1C:\shepherdump 








WINSGCK_DLL: Microsoft Windows Sockets Version 1.1. 
WS_FTP95 LE 4.04 96.08.21, Copyright © 1992-1996 Ipswitch, Inc. All rights reserved. 


LogWnd Help 


A Start || Figws_FTP95 LE | S® 1:21AM 


Figure 5: Windows WS_FTP95 Tool 





244 


fi WS_FTP95 LE 131.120.1.13 
Local Syste 


Remote Syst 
C:\shepherdump % /tmp_mnt/“users/’res/”shepherc ¥ 


shepherd . TXT 
US_FTP.LOG 
=) [-a-] 
co! 
97¢ 
shepherd 971 
shepherd.c 97 c | 
Rename shepherd .h 976 
shepherd. h~ 970 
shepherd.map 97([ 
shepherd.o 970 
‘63 shepherd .Ti 97( 


es] Simulator 97+] 
> 


€ ASCII ( Binary T Auto 


Received 4478 bytes in 0.4 secs, (109.14 Kbps], transfer succeeded 
226 Transfer complete. 


Close | Cancel | Logwnd | Help [Options | About _| Exit 


Asta ||Fpws_FTP95 LE 131.1... 8® 1:23AM 





Figure 6: WS_FTP95 Tool File Transfer 


16. The file is now on the "hard" disk of the laptop. You can now close the window or 
kill the process by clicking on the appropriate button (active window:upper right corner 
ited = ="’). 
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Use Windows 95 HyperTerminal Program for Direct Connection 


17. You are now back at the Windows desk top. Now double click on the ses2download 
shortcut to open a hard-line, under Windows 95 HyperTerminal (See figure 7). 


My Computer My Briefcase 
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ae 
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aioe al 
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Me Bie cerse Shartcut to 
i. ses2download 





Start D> 1:20AM 


Figure 7: Windows ses2download Shortcut 


246 


18. The next window to appear will be the open HyperTerminal window (See Figure 8). 
Press the "reset" button on the OMNIBYTE, Taurus board. The Taurus bug (debugger) 
prompt will appear in the HyperTerminal window (See Figure 8). 





Be ses2download - HyperTerminal — 
File Edit View (Cal Transfer Help 


Dis] al) cis st 


Taurus Debugger/Diagnostics Version 3.2 - 11/28/95 
FPC passed test 
Taurus_Bug>bf 0 40000 0 
Effective address: OO000000 
Effective address: OOOS3FFFC 
Taurus_Bug> 
lo 





a 


A Start | Fip WS_FIPS5 LE 131.12011.... [8 ses2download - Hype... S& 1:254M 


Figure 8: Windows HypertTerminal Window 
19. Ensure the lever on the switch box is placed on console (this allows the console to 
emulate a VT220). 
20. At the Taurus bug prompt type "bf 0 40000 0” and press the enter key. This 


command is called block fill by the debugger it allows you to disable the parity error 
interrupt (PEJ) and prevents problems caused by uninitialized variables. See Figure 8. 


21. At the Taurus bug prompt type "lo" and press the enter key. The "lo" command 
initiates the download from the console. See Figure 8. 
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22. Next place the /ever on the switch box is placed on host (this makes possible the use 
of the RS232 protocol to download shepherd. TXT from c:\shepherdump to the Taurus 
board). 


23. Click on the HyperTerminal "Transfer "option and choose the "Send Text File". 
All the "Send Text File" parameters have been previously set, so there is no action to 
take in that regard. 

See Figure 9. 





“> ses2download - HyperT erminal 
File Edt View Cal BEELER Help 


Cle - Send Fie... 

Dig] alS): eee 

_ Capture Tex... 

| See File... bn 3.2 - 11/28/95 











Taurus Debugger El 
FPC passed test : 
Taurus Bug>bf 0 Capture to Printer 
Effective address: 00000000 
Effective address: OOOS3FFFC 
Taurus _Bug> 

lo 


see een eee 


P ww de @ bwost Cle to thes caceate vecobea 


; aan a — 
‘AStart| FTf WS_FTPS5 LE 131.120.1....[[ ses2download - Hype... B® 1:27AM 


Figure 9: Windows HypertTerminal Window 
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24. Now move to the root directory and select the c:\shepherdump directory, and double 
click on shepherd.TXT file. See figure 10. 






ae ses2download - HyperTerminal 






File Edit View (al Transfer Help 
Yast ee RA ES 


Look jn: [a shepherdump ~| ce pees a 








Filename: {shepherd 
Fides of type: [Text file (“. TXT) ~| Cancel | 


CP cere ee hen ol TROIS NA iy MTITAM lacna frais? féconi { [rape Pitt ThA [ff neay a0 Tons aah 


AA Start| Fie WS_FTPS5 LE 131.1201... ses2download - Hype... | SD 1:23AM 


Figure 10: Send File From shepherdump 


The download process is in motion. The "red" transmit light on the RS232 connector to 
the switch box will become faint while transmission is in progress. Once the 
transmission 1s complete the "red" transmit light on the RS232 connector to the switch 
box will become a constant red; the Hyperterminal window will pause during the 
transmission process. The Taurus bug prompt will appear in the HyperTerminal 
window after the transmission is complete. 


25. Ensure the lever on the switch box is placed on console (this allows the console to 
emulate a VT220). 
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25. Now type ''go"' at the Taurus bug prompt and press the enter key (see figure 11). 
The program that you have previously downloaded will be executed. 


“ls ses2download - HyperTerminal 
File Edt View Call Transfer Help 


los | 3|3| aQ[5] 













Taurus Debugger/Diagnostics Version 3.2 - 11/28/95 
FPC passed test 
Taurus_Bug>bf 0 40000 0 
Effective address: 00000000 
Effective address: OOOSFFFC 
Taurus_Bug> 
lo 


Taurus _Bug> 
go 


er eos eer (TE yr SP SPE rssh 


“ - . 7 4 =| 
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Figure 11: Taurus Bug Prompt Returns After Transmisssion Completion 
and the "go'' Command is given to Execute the Program. 
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After the ''go"' has been given and the execution begins the Shepherd Main Menu 
appears for your Selection. 


IE. ses2download - HyperT erminal 
File Edt Yiew Call Transfer Help 


Dls| al) 2s] 2! 




























SHEPHERD Main Menu (Last Update: 27 Feb 97) 
1---5§----0----5----0----§----0---- § 9-9 - 0-9 - - -§ - - - -0----5----0----§----0----5----0 


Please choose: Diagnostics 
(1) Stop 

(Z) Straight Motion (Autonomous) 

(3) Straight Motion by Joystick 

(4) XY-Motion by Joystick 

(S) Rotate 

(6) Sinusoidal Motion 

(?) Tornado (External Center of Rotation) 
(8) Tornado (Internal Center of Rotation) 
(9) Align Wheels 

(O) Exit Program 





RAR RL CTE ICO EE EEE A I ADEE AP 
i fantecieolerlo to J ALNMALON Fy TIAN Tarcna mae Peo i Tra ae Pri thé lfmen ea ‘Toes AAakaA 


@AStart| Fip WS_FTP95 LE 131.1 20.1...) ses2download - Hype... | SD 1:33AM 


Figure 12: Shepherd Main Menu 


Note: remember this is a quick guide and does not provided answers to questions 
conceming file size, debugger commands, and other requirements or constraints. 
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APPENDIX L: SENSING SIMULATION DATA 


Distance x Y theta psi 
0.000 0.400 0.000 0.001 2.355 
0.400 0.800 0.001 0.002 2.353 
0.800 1.200 0.002 0.005 2.352 
1.200 1.600 0.005 0.008 2.350 
1.600 2.000 0.008 0.011 2.348 
2.000 2.400 0.014 0.015 2.347 
2.400 2.800 0.021 0.020 2.345 
2.800 3.200 0.029 0.025 2.344 
3.200 3.600 0.041 0.031 2.342 
3.600 3.999 0.054 0.037 2.341 
4.000 4.399 0.070 0.043 2.339 
4.400 4.799 0.088 0.050 2.337 
4.800 5.198 0.110 0.057 2.336 
5.200 5.597 0.134 0.064 2.334 
5.600 5.996 0.161 0.072 2.333 
6.000 6.395 0.192 0.080 2.331 
6.400 6.794 0.225 0.088 2.330 
6.800 7.192 0.262 0.096 2.328 
7.200 7.590 0.302 0.105 2.326 
7.600 7.988 0.346 0.114 2.325 
8.000 8.385 0.393 0.123 2.323 
8.400 Bag) 0.444 0.131 2.322 
8.800 9.178 0.498 0.141 2.320 
9.200 9.574 0.556 0.150 2.319 
9.600 9.969 0.617 0.159 2.217 
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APPENDIX M: SENSING SIMULATION CODE (MAIN2.CC) 


// File: main2.cc 

// Name: Edward Mays 
// Sensing Simulation 

// Unix 

/1 GCC 

// Date: 26 August 1997 
// 

// Description 


if THIS PROGM SIMULATES THE MOVEMENT OF A SQARE OBJECT ALONG A 
MPATH. THE OBJECT’S PATH DIRECTION (THETA) IS CHANGING, AS JS THE 
HOBJECTS ORIENTATION (PSI). LINE TRACKING IS USED AND THE X-AXIS IS 
THE //REFERENCE LINE. THE REFERENCE LINE IS INCREMENTED BY 40 
UNITS IN THE 


#include <iostream.h> 
#include <math.h> 
#include <fstream.h> 


#include <stdio.h> 


#define PI 3.14159265358979323846 
#define RAD 57.29577951308232087684 
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double deltaTime = 0.01;// 0.01 
double Vel = 40.0; 
double omega = -0.1570796327; 


FILE *f0, *f1, *f2, *f3, *f4, *f5,*f6; //PTR TO FILE FOR OUTPUT DATA 


//structure to hold configuration including x, y, theta, and kappa 


typedef struct{ 
double x; 
double y; } 
POINT; 


typedef struct{ 
POINT Point; 
double Theta; 
double Kappa; 
double Psi; 


CONFIGURATION: 


//Function: GetSmooth 


//Return Value:n/a 
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//Parameters: function parm list 


//Purpose: gets users input for sO/smoothness 


double GetSmooth(double &s0) 
{ 


cout << "enter your value for smoothness (negatives not allowed)" <<endl; 
cin >> sO; 


return (sO >= 0.0); 


\// GetSmooth 


//Function: InitConfig 
//Return Value:n/a 
//Parameters: function parm list 


//Purpose: SETS INITIAL CONFIGURATION 


void InitConfig(CONFIGURATION& q_init, CONFIGURATION& 
q_xaxis, CONFIGURATION& qbody, CONFIGURATION qfrontR, 


CONFIGURATION®& aqfrontL, CONFIGURATION& qrearR, CONFIGURATION& 
qrearL, CONFIGURATION qsnapshot, 


double &s0, double &deltaS) 
{ 


cout<<"Setting the initial configuration" 
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<<"x=0, y = 0, theta = 0, and kappa = 0 "<<end1; 


q_init.Point.x = 0.0; 
q_init.Point.y = 0.0; 
q_init.Theta = 0.0; 
q_init.Kappa = 0.0; 
qg_init.Psi = 2.356219449; /* 3*PI/4.0 */ 


cout<<"Setting the reference line configuration" 


<<"x = 0, y = 40, theta = 0 , and kappa = 0 "<<end; 


q_xaxis.Point.x = 0.0; 
q_xaxis.Point.y = 40.0; 
q_xaxis. Theta =0.0; 
q_xaxis.Kappa = 0.0; 


//individual wheels 
qfrontR.Point.x = 40; /* wheell */ 


qfrontR.Point.y = -40; 


qfrontL.Point.x = 40; /* wheel2 */ 


qfrontL.Point.y = 40; 


qrearR.Point.x =-40; /* wheel3 */ 


qrearR.Point.y = -40; 
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qrearL.Point.x = -40; /* wheel 4 */ 


qrearL.Point.y = 40; 


qsnapshot.Point.x = 0.0; 


qsnapshot.Point.y = 0.0; 


cout<<"Enter size constant for smoothness <return>"<<endl; 


GetSmooth(s0); 


cout<<"Entering Step size constant deltaS(deltaS=Vel*deltaT)."<<endl]; 


deltaS = Vel*deltaTime;//.05 orig 


}// InitConfig 


//Function: CreateConst 
/fReturn Value:n/a 

//Parameters: function parm list 
//Purpose: create constants for 


// steering function dk/ds 


void CreateConst(double &a, double &b, double &c, double &s0) 
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{ 
double k; 


k = 1.0/sO; //all consts by def, including curvature 
a=3.07& 

b= 3. 0ek+k; 

Gk * kak 


\// CreateConst 


//Function: GetSteerL 
//Return Value:n/a 


//Parameters: function parm list 


//Purpose: lambda=dk/ds (LINEAR STEERING FUNCTION) 


double GetSteerL(double &a, double &b, double &c, CONFIGURATION gq, 


CONFIGURATION q_xaxis) 


Zo) 


double delta_r; 


delta_r = -(q.Point.x - q_xaxis.Point.x)*sin(q_xaxis. Theta) + 


(q.Point.y - q_xaxis.Point.y)*cos(q_xaxis.Theta); 


return (-(a*q.Kappa + b*(q.Theta - q_xaxis.Theta) + c*delta_r)); 


} //GetSteerL 


//Function: GetDeltakappa 
/fReturn Value:n/a 
//Parameters: function parm list 


//Purpose: DETERMINES THE KAPPA DIFFERENCE PER INCREMENT OF S 


double GetDeltakappa(double &Dk_Ds, double &deltaS, double &deltakappa) 


{ 
deltakappa = Dk_Ds*deltaS; 


return(deltakappa); 


}//GetDeltakappa 
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//Function: returnkappa 
//Return Value:n/a 
//Parameters: function parm list 


//Purpose: CALCULATES NEW VALUE FOR KAPPA USING deltaK 


CONFIGURATION returnkappa(double &deltakappa, CONFIGURATION &q) 


{ 
q.Kappa = q.Kappa + deltakappa; 


return q; 


}//returnkappa 


//Function: GetS 
//Return Value:n/a 
//Parameters: function parm list 


//Purpose: INCREMENTS S THROUGH EACH ITERATION OF THE WHILE LOOP 


double GetS(double &s, double &deltaS) 
{ 


Ze 


s=s+deltaS; 


return s; 


\//GetS 


//Function: GetDeltaTheta 
//Return Value:n/a 
//Parameters: function parm list 


//Purpose: COMPUTES CHANGE IN THETA PER INCREMENT OF S 


double GetDeltaTheta; CONFIGURATION &q, double &deltaS, double &deltaT) 


deltaT = q.Kappa*deltaS; 


return(deltaT); 


}//GetDeltaTheta 


//Function: Circ 


/fReturn Value:n/a 


//Parameters: function parm list 


//Purpose: Circ function from notes 6.29 


215) 


void Circ(double Length, double alpha, CONFIGURATION &q) 
{ 

double alpha2, alpha4; 

alpha2=alpha*alpha; 

alpha4=alpha2*alpha2; 


//configuration gq] 
q.Point.x = (1.0 - alpha2/6.0 + alpha4/120.0)*Length; 
q.Point.y = (0.5 - alpha2/24.0 + alpha4/720.0)*Length*alpha; 


q.Theta = alpha; 


}//Circ 


//Function: Compose 
//Return Value:n/a 
//Parameters: function parm list 


//Purpose: updates the configuration and computes new config (notes 6.2) 


CONFIGURATION Compose(CONFIGURATION& q!,CONFIGURATION& 
q2, CONFIGURATION& gq3, double& s,double& deltaTime) 
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{ double x,y, 
sinTheta = sin(q!1.Theta), 


cosTheta = cos(q1.Theta); 


x = q1.Point.x + q2.Point.x*cosTheta - q2.Point.y*sinTheta; 
y = q1.Point.y + q2.Point.x*sinTheta + q2.Point.y*cosTheta; 
q3.Point.x = x; 

q3.Point.y = y; 

q3.Theta = q1.Theta + q2.Theta; 


q3.Psi = q1.Psi + (omega * deltaTime); /* how to handle move left/right? */ 


fprintf(f6,"%10.3f %10.3f %10.3f %10.3f %10.3f\n", 
s,q3.Point.x, q3.Point.y,q3. Theta, q3.Ps1); 


return q3; 


}// end Compose 


CONFIGURATION Compose2(CONFIGURATION& ql,CONFIGURATION& q?, 
CONFIGURATION& q3) /*position */ 


{ double x,y, 
sinTheta = sin(q1.Psi), 


cosTheta = cos(q!.Psi); 
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x = q1.Point.x + q2.Point.x*cosTheta - q2.Point.y*sinTheta; 


y = q1.Point.y + q2.Point.x*sinTheta + q2.Point.y*cosTheta; 


q3.Point.x = x; 
q3.Point.y = y; 
retum q3; 


}// end Compose2 


//Function: Openfile 
//Return Value:n/a 
//Parameters: function parm list 


//Purpose: To compute transposition 


void OpenfileQ) 

{ 

fO = fopen("drk.dat","w"); 

f1 = fopen("wheel1.dat","w"); 
f2 = fopen(“wheel2.dat","w"); 
f3 = fopen("wheel3.dat","w"); 
f4 = fopen("wheel4.dat","w"); 


{5 = fopen(""“composite.dat","w"); 


{6 = fopen("psi.dat","w"); 


\// Openfile 
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//Function: Print 
//Return Value:n/a 
//Parameters: function parm list 


//Purpose: To compute transposition 


void printFile(FILE *f, CONFIGURATION &q) 
{ 


fprintf(f,"%10.3f %10.3f\n", 


q.Point.x, q.Point.y); 


\// printFile 


//Function: blankLine 
//Return Value:n/a 
//Parameters: function parm list 


//Purpose: To compute transposition 


void blankLine(FILE *f) 
{ 
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fprintf(f,"\n"); 


\// blankLine 


//Function: updateWheels 
//Return Value:n/a 

//Parameters: function parm list 
/Purpose: create constants for 


// steering function dk/ds 


void updateWheels(CONFIGURATION& gqbody, CONFIGURATION&  qfrontR, 
CONFIGURATION qfrontL, CONFIGURATION qrearR, CONFIGURATION qrearL, 
CONFIGURATION& qwheell, CONFIGURATION& qwheel2, CONFIGURATION& 
qwheel3, CONFIGURATION& qwheel4, CONFIGURATIONS q3,int& s2) 


{ printFile(f0,qbody); 


qwheel! = Compose2(qbody,qfrontR,q3); 
printFile(f1,qwheel |); 

blankLine(f1); 

qwheel2 = Compose2(qbody,qfrontL,q3); 
printFile(f2,qwheel2); 
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blankLine(f2); 

qwheel3 = Compose2(qbody, qrearR, q3); 
printFile(f3,qwheel3); 

blankLine(f3); 

qwheel4 = Compose2(qbody, qrearL,q3); 
printFile(f4,qwheel4); 

blankLine(f4); 


if((s2==0.0)II(s2% 100==0)){ 


printFile(f5,qwheel1); 
printFile(fS5,qwheel2); 
printFile(f5,qwheel4); 
printFile(f5,qwheel3); 
printFile(f5,qwheel 1); 
blankLine(f5); 

}// updateWheels 


int main() 


Zo 


{ CONFIGURATION gq, q_xaxis, New_q, qbody, qfrontR, qfrontL, qrearR, qrearL, 
qwheell, qwheel2, qwheel3, qwheel4, qsnapshot,q3; 


int ix,s2,counter; 

double a, b, c; //constants equation 6.3 

double s,sO; //sO is smoothness, s is the incremental step 
double Dk_Ds; 

double deltaS; 

double deltaT; 

double deltaK; 

const double Sdig = 0.001; //const used for prec/toler 


double smax=400.0; 


InitConfig(q, q_xaxis, qbody, qfrontR, qfrontL, qrearR, qrearL,qsnapshot,sO, deltaS); 
//configuaration set up 


Openfile(); 

printFile(f0,q); 

printFile(fl,qfrontR); //write initial config to file 
printFile(f2,qfrontL); 

printFile(f3,qrearR); 

printFile(f4,qrearL); 


//printFile(f5,qfrontR); 
//printFile(f5,qfrontL); 
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//printFile(f5,qrearL); 
//printFile(f5,qrearR); 


printFile(f5,qfrontR); 
blankLine(f5); 


CreateConst(a, b, c, sO); //calcs consts 
Ss 0.0; 

SSS: 

counter=0; 

forax=0; ix<10;ix++){ 

do 

{// 


Dk_Ds = GetSteerL(a, b, c, q, q_xaxis); //calculates lambda =dk/ds 
deltaK = GetDeltakappa(Dk_Ds, deltaS, deltaK); /lambda*deltaS 
returnkappa(deltaK, q); //Kappa <= kappa + deltaK 
deltaT = GetDeltaTheta(q, deltaS, deltaT); //Theta <= Theta + deltaT 
Circ(deltaS, deltal, New_q); /Icir 
qbody = Compose(q, New_g, q, s, deltaTime); //compose 
update Wheels(qbody,qfrontR, gfrontL, qrearR, 

qrearL,qwheel!, qwheel2, qwheel3, 

qwheel4,q3,s2); 
GetS(s, deltaS); 


co=S* 


28] 


}while (s<smax); 

// }while ((fabs(q.Point.y) > Sdig)Il(fabs(q. Theta) > Sdig) Il 
// (fabs(q.Kappa) > Sdig)); 

s=0.0; 

S2=s; 


q_xaxis.Point.y = q_xaxis.Point.y + 40.0; 


if(ix%2==0){ 
Gexaxisslheta = FI, 
Gineta = PE 
omega = fabs(omega); 
Jelse { 

q_xaxis.Theta = 0.0; 
q.Theta =0.0; 
omega = -omega; 

} 

} 

fclose(f0); 

fclose(f1); 

fclose(f2); 

fclose(f3); 

fclose(f4): 

fclose(f5); 

fclose(f6); 

return 0; 


}//end main2.cc 
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-1024 -87.657 


-1023 -87.429 


-1020 -86.975 


-1015 -86.747 


-1010 -86.406 


-1005 -85.838 


-1000 -85.383 


-900 -76.856 


-800 -67.998 


-700 -59.082 


-600 -51.048 


-500 -42.748 


-400 -34.107 


-375 -31.834 


-350 -29.673 


-300 -25.580 


-250 -21.260 


-225 -19.100 


-200 -17.053 


APPENDIX N: INPUT VS. OUPUT VELOCITY 
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-175 -14.780 


-150 -12.506 


-125 -10.459 


-100 -8.413 


-90 -7.503 


-80 -6.707 


-70 -5.798 


-60 -5.002 


-50 -4.092 


-40 -3.297 


-30 -2.387 


-20 -1.591 


-10 -.682 


-5 -.34] 


-1 -.113 


0.0 0.0 


1023 87.429 


1020 86.975 


1015 86.747 


1010 86.406 


1005 85.838 


1000 85.383 
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900 76.856 


800 67.998 


700 59.082 


600 51.048 


500 42.748 


400 34.107 


S4973 12834 


550 29.673 


300 25.580 


250 21.260 


225 19.100 


200 17.053 


175 14.780 


150 12.506 


125 10.459 


100 8.413 


90 7.503 


80 6.707 


70 5.798 


60 5.002 


50 4.092 


40 3.297 
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30 2.387 


20 1.591 


10 .682 


5 .341 


lea 
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APPENDIX O: INPUT VS OUTPUT STEERING RATES 


A. DESIRED INPUT RATE VS ACTUAL (BOTH ESTIMATED AND 
SOFTWARE DEPENDENT) 


Desired rate of Time Estimated Rate Software 
turn Stop watch (rad/s) Measured Rate 


rad/s, average) 


(sec) 

| 60 | to0000 | stg 
| 169 | tne | 3.90653 
No data 


Figure A.1 Inputs and results from massaged data (error). No data entries exist because the 
revolutions were too fast for hand timing. 





B. DESIRED INPUT RATE VS OUPUT FOR EACH WHEEL (SOFTWARE 
DEPENDENT 


Below M5, M6, M7, and M8 reperesent the steering motors for wheel 1, wheel 2, 
wheel3, and wheel4 respectively. 
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of turn 


5.1035 5.0935 5.093 5.0955 
5.2065 5.198 5.1955 5.198 

5.238 5.235 5.234 5.235 
5.238 5.235 5.234 5.235 
2 
3 
5 










pe | 2.001 | 2.003 | 2.003 | 2.008 
| 3 | 30055 | 3.006 | 3.0085 | 3.006 
4 | 4007 | 4.006 | 40015 | 4.008 
os | so | so | 5.009 | 5010 


Figure A.2: Desired (commanded) rate of turn vs. actual “free floating’’ motor rate. 









C, DESIRED INPUT (DIGIT MANIPULATION) VS OUTPUT RATE 





ose | as | ota 
- 20 | ton ton | tt 
Pe 
| ao | tos | tos | zs 
so | ot | so |e 
306 
11 


045 

100 
143 143 
203 203 
250 248 
49 | gos | tom | to 
wo | sm S| sto | sto | 
. 





1.022 1.022 
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(digits) (rad/s (rad/s) 
. 


Figure A.3: Desired (commanded) rate of turn vs. actual ‘free floating” motor rate for each wheel 
using input digits . 





D. WHEEL 4 ROTATION DATA 


Clockwise Rotation | Counterclockwise 
Rotation 


1 000.867 360.390 
2 000.878 360.390 
3 000.976 360.363 











i 
haz 
a3 
4 | 090.933 | 36041 
6 | 0.992 | 360394 
| 8 | 902 | 360.394 
9 | 000.996 | 360.445 
/ to || 000.996 | 360.476 
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000.996 360.433 
000.863 360.472 
000.968 360.398 
000.941 360.425 


000.957 360.480 
000.937 360.480 


Figure A.4: Wheel 4 data based on position of rest after direction of turn. 


Clockwise Rotation | Counterclockwise 
Rotation 
001.003 360.468 


000.917 360.417 
000.972 360.468 
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